diff --git a/.gitmodules b/.gitmodules index c7b791a..e804f2e 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,12 +1,9 @@ -[submodule "coppeliarobotics/zmqRemoteApi"] - path = coppeliarobotics/zmqRemoteApi - url = https://github.com/CoppeliaRobotics/zmqRemoteApi.git -[submodule "jsoncons"] - path = jsoncons +[submodule "submodules/jsoncons"] + path = submodules/jsoncons url = https://github.com/danielaparker/jsoncons.git -[submodule "cppzmq"] - path = cppzmq - url = https://github.com/zeromq/cppzmq [submodule "submodules/cppzmq"] path = submodules/cppzmq - url = https://github.com/zeromq/cppzmq + url = https://github.com/zeromq/cppzmq.git +[submodule "submodules/zmqRemoteApi"] + path = submodules/zmqRemoteApi + url = https://github.com/CoppeliaRobotics/zmqRemoteApi.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 3335522..1e57ab5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,36 +1,37 @@ cmake_minimum_required(VERSION 3.5) -project(dqrobotics-interface-coppeliasim LANGUAGES CXX) +project(dqrobotics-interface-coppeliasim-zmq LANGUAGES CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) include(${CMAKE_CURRENT_SOURCE_DIR}/dependencies.cmake) set(SUBMODULES ${CMAKE_CURRENT_SOURCE_DIR}/submodules) -set(ZMQ_REMOTE_API_PATH ${CMAKE_CURRENT_SOURCE_DIR}/coppeliarobotics/zmqRemoteApi/) +set(ZMQ_REMOTE_API_PATH ${SUBMODULES}/zmqRemoteApi/) set(SOURCE_DIR ${ZMQ_REMOTE_API_PATH}/clients/cpp) INCLUDE_DIRECTORIES(${PROJECT_NAME} include) -INCLUDE_DIRECTORIES(${PROJECT_NAME} /coppeliarobotics/zmqRemoteApi/clients/cpp) +INCLUDE_DIRECTORIES(${PROJECT_NAME} ${SUBMODULES}/zmqRemoteApi/clients/cpp) + + add_library(${PROJECT_NAME} SHARED - src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterface.cpp - src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.cpp - src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimModels.cpp - src/dqrobotics/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.cpp - src/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimRobot.cpp - src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimRobot.cpp - coppeliarobotics/zmqRemoteApi/clients/cpp/RemoteAPIClient.cpp - ) + src/dqrobotics/interfaces/coppeliasim/internal/_zmq_wrapper.cpp + src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ.cpp + src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQExperimental.cpp +# src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.cpp +# src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.cpp +# src/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.cpp +# src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.cpp + submodules/zmqRemoteApi/clients/cpp/RemoteAPIClient.cpp + + ) target_compile_definitions(${PROJECT_NAME} PUBLIC -DSIM_REMOTEAPICLIENT_OBJECTS) -target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_BINARY_DIR}/jsoncons/include) +target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/jsoncons/include) target_link_libraries(${PROJECT_NAME} - #Boost::filesystem - #Boost::format - #Boost::program_options - ${CUSTOM_BOOST_COMPONENTS} - cppzmq + ${Boost_LIBRARIES} + zmq dqrobotics ) @@ -42,7 +43,7 @@ set_source_files_properties(RemoteAPIClient.cpp OBJECT_DEPENDS ${SOURCE_DIR}/Rem SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER - "include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterface.h" + "include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ.h" ) INSTALL(TARGETS ${PROJECT_NAME} @@ -50,11 +51,6 @@ INSTALL(TARGETS ${PROJECT_NAME} PUBLIC_HEADER DESTINATION "include/dqrobotics/interfaces/coppeliasim" PERMISSIONS OWNER_READ OWNER_WRITE GROUP_READ WORLD_READ) -#if(APPLE) -# TARGET_LINK_LIBRARIES(${PROJECT_NAME} -# -ldqrobotics) -#endif() - if(WIN32) INSTALL(TARGETS ${PROJECT_NAME} # https://stackoverflow.com/questions/21592361/cmake-install-is-not-installing-libraries-on-windows @@ -72,15 +68,15 @@ endif() # Other Headers INSTALL(FILES - include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.h - include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimModels.h - include/dqrobotics/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.h + include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.h + include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQExperimental.h +# include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.h DESTINATION "include/dqrobotics/interfaces/coppeliasim") # Robots INSTALL(FILES - include/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimRobot.h - include/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimRobot.h +# include/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.h +# include/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.h DESTINATION "include/dqrobotics/interfaces/coppeliasim/robots") ################################################################ @@ -89,39 +85,16 @@ INSTALL(FILES # base folder INSTALL(FILES - src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterface.cpp - src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimModels.cpp - src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.cpp - src/dqrobotics/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.cpp + src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ.cpp + src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQExperimental.cpp +# src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.cpp +# src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.cpp DESTINATION "src/dqrobotics/interfaces/coppeliasim") # robots folder INSTALL(FILES - src/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimRobot.cpp - src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimRobot.cpp +# src/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.cpp +# src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.cpp DESTINATION "src/dqrobotics/interfaces/coppeliasim/robots") -# Temporal Only for Test -#add_executable(example src/main.cpp) -#target_link_libraries(example PRIVATE ${PROJECT_NAME}) - -#add_executable(example2 src/test_library.cpp) -#target_link_libraries(example2 PRIVATE ${PROJECT_NAME})#dqrobotics dqrobotics-interface-coppeliasim) - - -#add_executable(example3 src/test_new_methods.cpp) -#target_link_libraries(example3 PRIVATE -# dqrobotics -# ${PROJECT_NAME})#dqrobotics dqrobotics-interface-coppeliasim) - -#add_executable(multiple_instances src/test_panda.cpp) -#target_link_libraries(multiple_instances PRIVATE -# #dqrobotics -# ${PROJECT_NAME}) - - -# Now simply link against gtest or gtest_main as needed. Eg -add_executable(unit_tests src/unit_tests.cpp) -target_link_libraries(unit_tests gtest_main ${PROJECT_NAME}) -#add_test(NAME example_test COMMAND example) diff --git a/README.md b/README.md index 53e1ba0..1f39d38 100644 --- a/README.md +++ b/README.md @@ -1,34 +1,17 @@ -![Static Badge](https://img.shields.io/badge/status-experimental-critical)![Static Badge](https://img.shields.io/badge/Platform-Apple_silicon-magenta)![Static Badge](https://img.shields.io/badge/Tested-Apple)![Static Badge](https://img.shields.io/badge/Platform-Ubuntu_x64-orange)![Static Badge](https://img.shields.io/badge/tested-green)![Static Badge](https://img.shields.io/badge/Platform-Windows_11-blue)![Static Badge](https://img.shields.io/badge/tested-green)![Static Badge](https://img.shields.io/badge/CoppeliaSim-4.8.0--rev0-orange)![Static Badge](https://img.shields.io/badge/Written_in-C%2B%2B17-blue)![GitHub License](https://img.shields.io/github/license/juanjqo/cpp-interface-coppeliasim)![Static Badge](https://img.shields.io/badge/based_on-ZeroMQ_remote_API-blue) +![Static Badge](https://img.shields.io/badge/status-experimental-critical)![Static Badge](https://img.shields.io/badge/Platform-Apple_silicon-magenta)![Static Badge](https://img.shields.io/badge/Tested-Apple)![Static Badge](https://img.shields.io/badge/Platform-Ubuntu_x64-orange)![Static Badge](https://img.shields.io/badge/tested-green)![Static Badge](https://img.shields.io/badge/CoppeliaSim-4.8.0--rev0-orange)![Static Badge](https://img.shields.io/badge/Written_in-C%2B%2B17-blue)![GitHub License](https://img.shields.io/github/license/dqrobotics/cpp-interface-coppeliasim-zmq)![Static Badge](https://img.shields.io/badge/based_on-ZeroMQ_remote_API-blue) +# cpp-interface-coppeliasim-zmq +A DQ Robotics interface based on the ZeroMQ remote API to connect with CoppeliaSim. This API provides more functionalities than the legacy remote API (the one used by the [DQ Robotics interface](https://github.com/dqrobotics/cpp-interface-vrep)). +| ![Static Badge](https://img.shields.io/badge/CoppeliaSim-4.8.0--rev0-orange) | SO | Status (C++17) | +| ------------- | ------------- |------------- | +| ![Static Badge](https://img.shields.io/badge/Apple_silicon-magenta)| macOS ![Static Badge](https://img.shields.io/badge/Apple_silicon-magenta) | ![Static Badge](https://img.shields.io/badge/beta-yellow)| +| ![Static Badge](https://img.shields.io/badge/x64-blue) ![Static Badge](https://img.shields.io/badge/arm64-blue) | Ubuntu {22.04, 24.04} LTS ![Static Badge](https://img.shields.io/badge/x64-blue) ![Static Badge](https://img.shields.io/badge/arm64-blue) | ![Static Badge](https://img.shields.io/badge/beta-yellow)| +| ![Static Badge](https://img.shields.io/badge/x64-blue) ![Static Badge](https://img.shields.io/badge/arm64-blue) | Windows 11 ![Static Badge](https://img.shields.io/badge/x64-blue) ![Static Badge](https://img.shields.io/badge/arm64-blue) | ![Static Badge](https://img.shields.io/badge/unsupported-gray) | -# dqrobotics-interface-coppeliasim (Matlab ≥ 2023b, C++17, and CoppeliaSim ≥ v4.7.0-rev0) - -An **unofficial** DQ Robotics interface to connect with CoppeliaSim based on ZeroMQ remote API. This API provides more functionalities when compared to the legacy remote API (the one used by the [DQ Robotics interface](https://github.com/dqrobotics/cpp-interface-vrep)). However, unlike DQ Robotics, dqrobotics-interface-coppeliasim is experimental and lacks official support. - -| ![Static Badge](https://img.shields.io/badge/CoppeliaSim-4.8.0--rev0-orange) | SO | Status (C++17) | Status (Python) | Status (Matlab ≥ R2023b) | -| ------------- | ------------- |------------- |------------- |------------- | -| ![Static Badge](https://img.shields.io/badge/Apple_silicon-magenta)| macOS ![Static Badge](https://img.shields.io/badge/Apple_silicon-magenta) | ![Static Badge](https://img.shields.io/badge/beta-yellow)|![Static Badge](https://img.shields.io/badge/unsupported-gray)|![Static Badge](https://img.shields.io/badge/pre--alpha-red)| -| ![Static Badge](https://img.shields.io/badge/x64-blue) ![Static Badge](https://img.shields.io/badge/arm64-blue) | Ubuntu {22.04, 24.04} LTS ![Static Badge](https://img.shields.io/badge/x64-blue) ![Static Badge](https://img.shields.io/badge/arm64-blue) | ![Static Badge](https://img.shields.io/badge/beta-yellow)|![Static Badge](https://img.shields.io/badge/unsupported-gray)|![Static Badge](https://img.shields.io/badge/pre--alpha-red)| -| ![Static Badge](https://img.shields.io/badge/x64-blue) ![Static Badge](https://img.shields.io/badge/arm64-blue) | Windows 11 ![Static Badge](https://img.shields.io/badge/x64-blue) ![Static Badge](https://img.shields.io/badge/arm64-blue) | ![Static Badge](https://img.shields.io/badge/pre--alpha-red) | ![Static Badge](https://img.shields.io/badge/unsupported-gray)|![Static Badge](https://img.shields.io/badge/pre--alpha-red)| - - -# Don't use this interface for your project! :warning: - -![Static Badge](https://img.shields.io/badge/warning-yellow) -This project is under active development, incomplete, and experimental/unstable. Therefore, **I highly recommend** the [official DQ Robotics interface](https://github.com/dqrobotics/cpp-interface-vrep) if you want stability and outstanding technical support. - -| Feature | Status (C++17) | Status (Python) | Status (Matlab ≥ R2023b) | -| -------- | -------------- |---------------- |-------------------------- | -| Implementation | :construction: under construction :rocket:| -- | :construction: under construction :turtle: | -| Documentation | :construction: under construction :rocket: | -- | :pushpin: planned | -| Unit Testing | :construction: under construction :rocket: | -- | :pushpin: planned| -| Packages | :pushpin: planned | -- | :pushpin: planned | - -**The C++17 version is the baseline, and you could expect it to be the most stable, reliable, and better-supported version.** - +# Instructions for Developers ## Basic requirements (for C++ users) @@ -86,14 +69,14 @@ Instructions missing here! ### MacOS (Apple Silicon) ```shell -brew install pkg-config cppzmq eigen +brew install pkg-config cppzmq eigen boost ``` ### Ubuntu ```shell -sudo apt install libzmq3-dev +sudo apt install libzmq3-dev libboost-all-dev ``` ### Windows @@ -107,11 +90,11 @@ Required vcpkg packages: ## Build and Install (UNIX) -Example for coppeliasim-v4.8.0-rev0. Note: :warning: replace coppeliasim-v4.8.0-rev0 with the actual CoppeliaSim version you have (≥ v4.7.0-rev0). +Example for coppeliasim-v4.8.0-rev0. Note: :warning: replace coppeliasim-v4.8.0-rev0 with your CoppeliaSim version (≥ v4.7.0-rev0). ```shell -git clone https://github.com/juanjqo/dqrobotics-interface-coppeliasim --recursive -cd dqrobotics-interface-coppeliasim/coppeliarobotics/zmqRemoteApi +git clone https://github.com/dqrobotics/cpp-interface-coppeliasim-zmq.git --recursive +cd cpp-interface-coppeliasim-zmq/submodules/zmqRemoteApi git checkout coppeliasim-v4.8.0-rev0 cd ../.. mkdir build && cd build @@ -146,8 +129,6 @@ cmake --install . ``` - - # Example (Find more examples [here](https://github.com/juanjqo/dqrobotics-interface-coppeliasim-examples)) 1) Open CoppeliaSim. (You do not need to load a specific scene). @@ -157,66 +138,55 @@ cmake --install . ```cpp #include -#include -#include +#include using namespace DQ_robotics; using namespace Eigen; -VectorXd compute_control_signal(const MatrixXd& J, - const VectorXd& q, - const double& damping, - const double& gain, - const VectorXd& task_error); int main() { - auto vi = std::make_shared(); + auto vi = std::make_shared(); vi->connect(); + //To enable experimental methods // Load the models only if they are not already on the scene. vi->load_from_model_browser("/robots/non-mobile/UR5.ttm", "/UR5"); vi->load_from_model_browser("/other/reference frame.ttm", "/Current_pose"); vi->load_from_model_browser("/other/reference frame.ttm", "/Desired_pose"); + + + auto jointnames = vi->get_jointnames_from_parent_object("/UR5"); + vi->set_joint_modes(jointnames, DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_MODE::DYNAMIC); + vi->set_joint_control_modes(jointnames, DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE::VELOCITY); + vi->enable_dynamics(true); + vi->set_engine(DQ_CoppeliaSimInterfaceZMQExperimental::ENGINE::MUJOCO); + vi->set_simulation_time_step(0.05); + vi->set_stepping_mode(true); + vi->draw_trajectory(jointnames.back(), 2, {1,0,1}, 1000); + vi->start_simulation(); - auto robot = URXCoppeliaSimRobot("/UR5", vi, URXCoppeliaSimRobot::MODEL::UR5); - auto robot_model = robot.kinematics(); - robot.set_robot_as_visualization_tool(); - auto q = robot.get_configuration_space_positions(); - double gain = 10; - double T = 0.001; - double damping = 0.01; + auto qd = (VectorXd(6)<< 0.5, 0.5, 0.5,0.5,0.5,0.5).finished(); + auto q = vi->get_joint_positions(jointnames); + VectorXd error = qd-q; + double k = 0.1; - auto xd = robot_model.fkm(((VectorXd(6) << 0.5, 0, 1.5, 0, 0, 0).finished())); - vi->set_object_pose("/Desired_pose", xd); + double epsilon = 0.1; - for (int i=0; i<300; i++) + while (error.norm() > epsilon) { - auto x = robot_model.fkm(q); - vi->set_object_pose("/Current_pose", x); - auto J = robot_model.pose_jacobian(q); - auto Jt = robot_model.translation_jacobian(J, x); - auto task_error = (x.translation()-xd.translation()).vec4(); - auto u = compute_control_signal(Jt, q, damping, gain, task_error); - q = q + T*u; - robot.set_control_inputs(q); - std::cout<<"error: "<get_joint_positions(jointnames); + error = qd-q; + auto u = k*error; + vi->set_joint_target_velocities(jointnames, u); + vi->trigger_next_simulation_step(); + std::cout<<"error: "<stop_simulation(); } -VectorXd compute_control_signal(const MatrixXd& J, - const VectorXd& q, - const double& damping, - const double& gain, - const VectorXd& task_error) -{ - VectorXd u = (J.transpose()*J + damping*damping*MatrixXd::Identity(q.size(), q.size())).inverse()* - J.transpose()*(-gain*task_error); - return u; -} ``` @@ -224,7 +194,7 @@ VectorXd compute_control_signal(const MatrixXd& J, add_executable(${CMAKE_PROJECT_NAME} main.cpp) target_link_libraries(${CMAKE_PROJECT_NAME} dqrobotics - dqrobotics-interface-coppeliasim) + dqrobotics-interface-coppeliasim-zmq) ``` diff --git a/boost_dependencies.cmake b/boost_dependencies.cmake deleted file mode 100644 index 0ec9486..0000000 --- a/boost_dependencies.cmake +++ /dev/null @@ -1,26 +0,0 @@ -# Download and extract the boost library from GitHub -# Add boost lib sources -set(BOOST_INCLUDE_LIBRARIES thread format filesystem system program_options) -set(BOOST_ENABLE_CMAKE ON) - -message(STATUS "Downloading dependencies. This will take some time...") -include(FetchContent) -Set(FETCHCONTENT_QUIET FALSE) # Needed to print downloading progress -FetchContent_Declare( - Boost - #URL https://github.com/boostorg/boost/releases/download/boost-1.84.0/boost-1.84.0.tar.gz - URL https://github.com/boostorg/boost/releases/download/boost-1.81.0/boost-1.81.0.tar.gz - USES_TERMINAL_DOWNLOAD FALSE - GIT_PROGRESS FALSE - DOWNLOAD_NO_EXTRACT FALSE -) -FetchContent_MakeAvailable(Boost) - -set(CUSTOM_BOOST_COMPONENTS - Boost::filesystem - Boost::format - Boost::program_options -) - - - diff --git a/dependencies.cmake b/dependencies.cmake index 838a87a..4aca8bc 100644 --- a/dependencies.cmake +++ b/dependencies.cmake @@ -8,25 +8,13 @@ endif(POLICY CMP0135) if(UNIX AND NOT APPLE) FIND_PACKAGE(Eigen3 REQUIRED) INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR}) - #find_package(cppzmq REQUIRED) - #find_package(ZeroMQ REQUIRED) ADD_COMPILE_OPTIONS(-Werror=return-type -Wall -Wextra -Wmissing-declarations -Wredundant-decls -Woverloaded-virtual) - - include(FetchContent) - set(CPPZMQ_BUILD_TESTS OFF CACHE BOOL "" FORCE) - FetchContent_Declare(cppzmq - GIT_REPOSITORY https://github.com/zeromq/cppzmq - SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/cppzmq - ) - FetchContent_GetProperties(cppzmq) - if(NOT cppzmq_POPULATED) - FetchContent_Populate(cppzmq) - add_subdirectory(${cppzmq_SOURCE_DIR} ${cppzmq_BINARY_DIR} EXCLUDE_FROM_ALL) - endif() - + + find_package(Boost 1.71.0 COMPONENTS filesystem system) + include_directories(${Boost_INCLUDE_DIRS}) endif() -if(APPLE) #APPLE +if(APPLE) INCLUDE_DIRECTORIES( /usr/local/include/ /usr/local/include/eigen3 @@ -40,10 +28,8 @@ if(APPLE) #APPLE /usr/local/lib/ /opt/homebrew/lib/ ) - find_package(cppzmq REQUIRED) find_package(ZeroMQ REQUIRED) - endif() @@ -62,73 +48,44 @@ if(WIN32) IMPORTED_LOCATION ${DQROBOTICS_PATH}/bin/dqrobotics.dll IMPORTED_IMPLIB ${DQROBOTICS_PATH}/lib/dqrobotics.lib INTERFACE_INCLUDE_DIRECTORIES ${DQROBOTICS_PATH}/include) -endif() - - -#-----This works but is super slow--------------# -#set(BOOST_INCLUDE_LIBRARIES thread format filesystem system program_options) -#set(BOOST_ENABLE_CMAKE ON) -# Download and extract the boost library from GitHub -#message(STATUS "Downloading and extracting boost library sources. This will take some time...") -#Set(FETCHCONTENT_QUIET FALSE) -#include(FetchContent) -#FetchContent_Declare( -# Boost -# GIT_REPOSITORY https://github.com/boostorg/boost.git -# GIT_TAG boost-1.84.0 -# USES_TERMINAL_DOWNLOAD TRUE -# GIT_PROGRESS TRUE -# GIT_SHALLOW TRUE -#) -#FetchContent_MakeAvailable(Boost) -#----------------------------------------------- - - -find_package(Boost) -if(Boost_FOUND) - #if (Boost_VERSION_MAJOR LESS_EQUAL 1 AND Boost_VERSION_MINOR LESS_EQUAL 81) - include_directories(${Boost_INCLUDE_DIRS}) - message(AUTHOR_WARNING "Local Boost ${Boost_VERSION_MAJOR}.${Boost_VERSION_MINOR}.${Boost_VERSION_COUNT} found!") - set(CUSTOM_BOOST_COMPONENTS - ${Boost_PROGRAM_FILESYTEM_LIBRARY} - ${Boost_PROGRAM_FORMAT_LIBRARY} - ${Boost_PROGRAM_OPTIONS_LIBRARY} - ) - #else() - # message(AUTHOR_WARNING "Local Boost ${Boost_VERSION_MAJOR}.${Boost_VERSION_MINOR}.${Boost_VERSION_COUNT} is not compatible. I'm going to download a compatible one!") - #endif() -else() - message(AUTHOR_WARNING "Local Boost not found. I'm going to download it!") - include(boost_dependencies.cmake) -endif() - - + find_package(Boost) + if(Boost_FOUND) + include_directories(${Boost_INCLUDE_DIRS}) + message(AUTHOR_WARNING "Local Boost ${Boost_VERSION_MAJOR}.${Boost_VERSION_MINOR}.${Boost_VERSION_COUNT} found!") + else() + message(AUTHOR_WARNING "Local Boost not found. I'm going to download it!") + + # Download and extract the boost library from GitHub + # Add boost lib sources + set(BOOST_INCLUDE_LIBRARIES thread format filesystem system program_options) + set(BOOST_ENABLE_CMAKE ON) + + message(STATUS "Downloading dependencies. This will take some time...") + include(FetchContent) + Set(FETCHCONTENT_QUIET FALSE) # Needed to print downloading progress + FetchContent_Declare( + Boost + #URL https://github.com/boostorg/boost/releases/download/boost-1.84.0/boost-1.84.0.tar.gz + URL https://github.com/boostorg/boost/releases/download/boost-1.81.0/boost-1.81.0.tar.gz + USES_TERMINAL_DOWNLOAD FALSE + GIT_PROGRESS FALSE + DOWNLOAD_NO_EXTRACT FALSE + ) + FetchContent_MakeAvailable(Boost) + #set(CUSTOM_BOOST_COMPONENTS + # Boost::filesystem + # Boost::format + # Boost::program_options + #) + endif() -include(FetchContent) -FetchContent_Declare(jsoncons - GIT_REPOSITORY https://github.com/danielaparker/jsoncons - SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/jsoncons -) -FetchContent_GetProperties(jsoncons) -if(NOT jsoncons_POPULATED) - FetchContent_Populate(jsoncons) - #add_subdirectory(${jsoncons_SOURCE_DIR} ${jsoncons_BINARY_DIR} EXCLUDE_FROM_ALL) endif() -message(STATUS "Dependencies ready!") +INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/submodules/cppzmq) +INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/submodules/jsoncons/include) -include(FetchContent) -FetchContent_Declare( - googletest - # Specify the commit you depend on and update it regularly. - #URL https://github.com/google/googletest/archive/5376968f6948923e2411081fd9372e71a59d8e77.zip - URL https://github.com/google/googletest/archive/refs/tags/v1.14.0.tar.gz -) -# For Windows: Prevent overriding the parent project's compiler/linker settings -set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) -FetchContent_MakeAvailable(googletest) diff --git a/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ.h b/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ.h new file mode 100644 index 0000000..cc203ca --- /dev/null +++ b/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ.h @@ -0,0 +1,212 @@ +/** +(C) Copyright 2024 DQ Robotics Developers + +This file is based on 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: +- Juan Jose Quiroz Omana + - Responsible for the original implementation. + The DQ_CoppeliaSimInterface class is partially based on the DQ_VrepInterface class + (https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_VrepInterface.h) + +*/ + +#pragma once +#include +#include +#include +#include +#include + + +using namespace DQ_robotics; +using namespace Eigen; + +class DQ_CoppeliaSimInterfaceZMQ: public DQ_CoppeliaSimInterface +{ +public: + DQ_CoppeliaSimInterfaceZMQ(); + ~DQ_CoppeliaSimInterfaceZMQ(); + + //-----------Override from DQ_CoppeliaSimInterface --------------------------------------- + bool connect(const std::string& host = "localhost", + const int& port = 23000, + const int&TIMEOUT_IN_MILISECONDS = 300) override; + + void start_simulation() const override; + void stop_simulation() const override; + void set_stepping_mode(const bool& flag) const override; + void trigger_next_simulation_step() const override; + + int get_object_handle(const std::string& objectname) override; + std::vector get_object_handles(const std::vector& objectnames) override; + DQ get_object_translation(const std::string& objectname) override; + void set_object_translation(const std::string& objectname, const DQ& t) override; + DQ get_object_rotation(const std::string& objectname) override; + void set_object_rotation(const std::string& objectname, const DQ& r) override; + void set_object_pose(const std::string& objectname, const DQ& h) override; + DQ get_object_pose(const std::string& objectname) override; + + VectorXd get_joint_positions(const std::vector& jointnames) override; + void set_joint_positions(const std::vector& jointnames, + const VectorXd& angles_rad) override; + void set_joint_target_positions(const std::vector& jointnames, + const VectorXd& angles_rad) override; + VectorXd get_joint_velocities(const std::vector& jointnames) override; + void set_joint_target_velocities(const std::vector& jointnames, + const VectorXd& angles_rad_dot) override; + void set_joint_torques(const std::vector& jointnames, + const VectorXd& torques) override; + VectorXd get_joint_torques(const std::vector& jointnames) override; + + std::string _get_object_name(const int& handle); + + template + std::vector _get_object_names(const T &handles) + { + int n = handles.size(); + std::vector objectnames(n); + for(auto i=0;i client_created_{false}; + + // If true, the class will accept names without a slash in the first character. + bool enable_deprecated_name_compatibility_{true}; + + void _check_client() const; + [[noreturn]] void _throw_runtime_error(const std::string& msg) const; + + int MAX_TIME_IN_MILLISECONDS_TO_TRY_CONNECTION_{300}; + double elapsed_time_ {0}; + std::thread chronometer_thread_; + void _join_if_joinable_chronometer_thread(); + void _start_chronometer(); + void _check_connection(); + bool _create_client(const std::string& host, + const int& rpcPort, + const int& cntPort, + const int& verbose_, + const bool& client_flag); + + void _set_status_bar_message(const std::string& message) const; + void __set_status_bar_message(const std::string &message, const int& verbosity_type) const; + + //-------------------map zone-------------------------------------------- + std::string _map_simulation_state(const int& state); + std::unordered_map handles_map_; + void _update_map(const std::string& objectname, const int& handle, const UPDATE_MAP& mode = UPDATE_MAP::ADD); + int _get_handle_from_map(const std::string& objectname); + + //------------------------------------------------------------------------ + std::string _remove_first_slash_from_string(const std::string& str) const; + bool _start_with_slash(const std::string& str) const; + std::string _get_standard_name(const std::string& str) const; + + //------------------Additional methods-------------------------------------------------------------// + DQ _get_object_translation(const int& handle) const; + void _set_object_translation(const int& handle, const DQ& t); + DQ _get_object_rotation(const int& handle) const; + void _set_object_rotation(const int& handle, const DQ& r); + DQ _get_object_pose(const int& handle) const; + void _set_object_pose(const int& handle, const DQ& h) const; + + double _get_joint_position(const int& handle) const; + double _get_joint_position(const std::string& jointname); + VectorXd _get_joint_positions(const std::vector& handles) const; + + void _set_joint_position(const int& handle, const double& angle_rad) const; + void _set_joint_position(const std::string& jointname, const double& angle_rad); + void _set_joint_positions(const std::vector& handles, const VectorXd& angles_rad) const ; + + void _set_joint_target_position(const int& handle, const double& angle_rad) const; + void _set_joint_target_position(const std::string& jointname, const double& angle_rad); + void _set_joint_target_positions(const std::vector& handles, const VectorXd& angles_rad) const; + + double _get_joint_velocity(const int& handle) const; + double _get_joint_velocity(const std::string& jointname); + VectorXd _get_joint_velocities(const std::vector& handles) const; + + void _set_joint_target_velocity(const int& handle, const double& angle_rad_dot) const; + void _set_joint_target_velocity(const std::string& jointname, const double& angle_rad_dot); + void _set_joint_target_velocities(const std::vector& handles, const VectorXd& angles_rad_dot) const; + + void _set_joint_torque(const int& handle, const double& torque) const; + void _set_joint_torque(const std::string& jointname, const double& torque); + void _set_joint_torques(const std::vector& handles, const VectorXd& torques) const; + + double _get_joint_torque(const int& handle) const; + double _get_joint_torque(const std::string& jointname); + VectorXd _get_joint_torques(const std::vector& handles) const; + + + template + void _check_sizes(const T &v1, + const U &v2, + const std::string& error_message) const + { + if (static_cast(v1.size()) != static_cast(v2.size())) + throw std::runtime_error(error_message); + } +}; + + + + + diff --git a/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterface.h b/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQExperimental.h similarity index 55% rename from include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterface.h rename to include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQExperimental.h index 151d9ec..a926b01 100644 --- a/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterface.h +++ b/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQExperimental.h @@ -16,6 +16,8 @@ This file is based on DQ Robotics. 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: - Juan Jose Quiroz Omana - Responsible for the original implementation. @@ -26,17 +28,25 @@ This file is based on DQ Robotics. #pragma once #include -#include +#include #include #include -//#include // For future use of C++23 features +#include +#include + using namespace DQ_robotics; using namespace Eigen; -class DQ_CoppeliaSimInterface + +class DQ_CoppeliaSimInterfaceZMQExperimental: public DQ_CoppeliaSimInterfaceZMQ { public: + enum class REFERENCE + { + BODY_FRAME, + ABSOLUTE_FRAME + }; enum class JOINT_MODE { KINEMATIC, @@ -61,11 +71,6 @@ class DQ_CoppeliaSimInterface CUSTOM, TORQUE }; - enum class REFERENCE - { - BODY_FRAME, - ABSOLUTE_FRAME - }; enum class PRIMITIVE { PLANE, DISC, @@ -81,134 +86,83 @@ class DQ_CoppeliaSimInterface ANY }; - DQ_CoppeliaSimInterface(); - ~DQ_CoppeliaSimInterface(); - - // Explicitly Deleted Assignment Operator - // DQ_CoppeliaSimInterface& operator=(const DQ_CoppeliaSimInterface&) = delete; - - // Explicitly Deleted Copy Constructor - // DQ_CoppeliaSimInterface(const DQ_CoppeliaSimInterface&) = delete; - - // Explicitly Deleted Default Constructor - // DQ_CoppeliaSimInterface() = delete; + DQ_CoppeliaSimInterfaceZMQExperimental(); + ~DQ_CoppeliaSimInterfaceZMQExperimental(); - bool connect(const std::string& host = "localhost", - const int& rpcPort = 23000, - const int& MAX_TIME_IN_MILLISECONDS_TO_TRY_CONNECTION = 300, - const int& cntPort = -1, - const int& verbose = -1); - void start_simulation() const; - void pause_simulation() const; - void stop_simulation() const; - void set_stepping_mode(const bool& flag); - double get_simulation_time() const; - void trigger_next_simulation_step() const; - bool is_simulation_running() const; - int get_simulation_state() const; - void set_status_bar_message(const std::string& message) const; - - int get_object_handle(const std::string& objectname); - std::vector get_object_handles(const std::vector& objectnames); - - - DQ get_object_translation(const int& handle) const; - DQ get_object_translation(const std::string& objectname); - - void set_object_translation(const int& handle, const DQ& t); - void set_object_translation(const std::string& objectname, const DQ& t); - - DQ get_object_rotation(const int& handle) const; - DQ get_object_rotation(const std::string& objectname); - - void set_object_rotation(const int& handle, const DQ& r); - void set_object_rotation(const std::string& objectname, const DQ& r); - - DQ get_object_pose(const int& handle) const; - DQ get_object_pose(const std::string& objectname); - - void set_object_pose(const int& handle, const DQ& h) const; - void set_object_pose(const std::string& objectname, const DQ& h); - - double get_joint_position(const int& handle) const; - double get_joint_position(const std::string& jointname); - VectorXd get_joint_positions(const std::vector& handles) const; - VectorXd get_joint_positions(const std::vector& jointnames); - - void set_joint_position(const int& handle, const double& angle_rad) const; - void set_joint_position(const std::string& jointname, const double& angle_rad); - void set_joint_positions(const std::vector& handles, const VectorXd& angles_rad) const; - void set_joint_positions(const std::vector& jointnames, const VectorXd& angles_rad); + void plot_reference_frame(const std::string& name, + const DQ& pose, + const double& scale = 1, + const std::vector& thickness_and_length = {0.005, 0.1}); - void set_joint_target_position(const int& handle, const double& angle_rad) const; - void set_joint_target_position(const std::string& jointname, const double& angle_rad); - void set_joint_target_positions(const std::vector& handles, const VectorXd& angles_rad) const; - void set_joint_target_positions(const std::vector& jointnames, const VectorXd& angles_rad); + void plot_plane(const std::string& name, + const DQ& normal_to_the_plane, + const DQ& location, + const std::vector& sizes = {0.2,0.2}, + const std::vector& rgba_color = {1,0,0,0.5}, + const bool& add_normal = true, + const double& normal_scale = 1); - double get_joint_velocity(const int& handle) const; - double get_joint_velocity(const std::string& jointname); - VectorXd get_joint_velocities(const std::vector& handles) const; - VectorXd get_joint_velocities(const std::vector& jointnames); + void plot_line(const std::string& name, + const DQ& line_direction, + const DQ& location, + const std::vector& thickness_and_length = {0.01,1.5}, + const std::vector& rgba_color = {1,0,0,0.5}, + const bool& add_arrow = true, + const double& arrow_scale = 1); - void set_joint_target_velocity(const int& handle, const double& angle_rad_dot) const; - void set_joint_target_velocity(const std::string& jointname, const double& angle_rad_dot); - void set_joint_target_velocities(const std::vector& handles, const VectorXd& angles_rad_dot) const; - void set_joint_target_velocities(const std::vector& jointnames, const VectorXd& angles_rad_dot); + void plot_cylinder(const std::string& name, + const DQ& direction, + const DQ& location, + const std::vector& width_and_length = {0.2,1.0}, + const std::vector& rgba_color = {1,0,0,0.5}, + const bool& add_line = true, + const double& line_scale = 1); - void set_joint_torque(const int& handle, const double& torque) const; - void set_joint_torque(const std::string& jointname, const double& torque); - void set_joint_torques(const std::vector& handles, const VectorXd& torques) const; - void set_joint_torques(const std::vector& jointnames, const VectorXd& torques); + void plot_sphere(const std::string& name, + const DQ& location, + const double& size = 0.2, + const std::vector rgba_color = {1,0,0,0.5}); - double get_joint_torque(const int& handle) const; - double get_joint_torque(const std::string& jointname); - VectorXd get_joint_torques(const std::vector& handles) const; - VectorXd get_joint_torques(const std::vector& jointnames); + bool load_model(const std::string& path_to_filename, + const std::string& desired_model_name, + const bool& load_model_only_if_missing = true, + const bool& remove_child_script = true); - std::string get_object_name(const int& handle); + bool load_from_model_browser(const std::string& path_to_filename, + const std::string& desired_model_name, + const bool& load_model_only_if_missing = true, + const bool& remove_child_script = true); - template - std::vector get_object_names(const T& handles); + int add_primitive(const PRIMITIVE& primitive, + const std::string& name, + const std::vector& sizes) const; - std::vector get_jointnames_from_parent_object(const std::string& parent_objectname); - std::vector get_shapenames_from_parent_object(const std::string& parent_objectname, - const SHAPE_TYPE& shape_type = SHAPE_TYPE::ANY); + bool object_exist_on_scene(const std::string& objectname) const; - VectorXd get_angular_and_linear_velocities(const int& handle, - const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME) const; + void remove_object(const std::string& objectname, + const bool& remove_children = false); - VectorXd get_angular_and_linear_velocities(std::string& objectname, - const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME); + void draw_trajectory(const std::string& objectname, + const double& size = 2, + const std::vector& rgb_color = {1,0,1}, + const int& max_item_count = 1000); - void set_angular_and_linear_velocities(const int& handle, - const DQ& w, - const DQ& p_dot, - const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME) const; - void set_angular_and_linear_velocities(std::string& objectname, - const DQ& w, - const DQ& p_dot, - const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME); - void set_twist(const int& handle, - const DQ& twist, - const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME) const; - void set_twist(const std::string& objectname, - const DQ& twist, const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME); - DQ get_twist(const int& handle, - const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME) const; - DQ get_twist(const std::string& objectname, - const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME); + bool check_collision(const std::string& objectname1, const std::string& objectname2); + std::vector get_bounding_box_size(const std::string& objectname); - double get_mass(const int& handle) const; - double get_mass(const std::string& object_name); + //-------Mujoco settings------------------------------------------------------------- + void set_mujoco_joint_stiffnesses(const std::vector& jointnames, + const double& stiffness); - DQ get_center_of_mass(const int& handle, const REFERENCE& reference_frame=REFERENCE::ABSOLUTE_FRAME) const; - DQ get_center_of_mass(const std::string& object_name, const REFERENCE& reference_frame=REFERENCE::ABSOLUTE_FRAME); + void set_mujoco_joint_dampings(const std::vector& jointnames, + const double& damping); - MatrixXd get_inertia_matrix(const int& handle, const REFERENCE& reference_frame=REFERENCE::BODY_FRAME); - MatrixXd get_inertia_matrix(const std::string& link_name, const REFERENCE& reference_frame=REFERENCE::BODY_FRAME); + void set_mujoco_joint_armatures(const std::vector& jointnames, + const double& armature); - //------------------Exclusive methods--------------------------------------------------------------- + void set_mujoco_body_frictions(const std::vector& bodynames, + const std::vector& friction); void set_joint_mode(const std::string& jointname, const JOINT_MODE& joint_mode); void set_joint_modes(const std::vector& jointnames, const JOINT_MODE& joint_mode); void set_joint_control_mode(const std::string& jointname, const JOINT_CONTROL_MODE& joint_control_mode); @@ -221,6 +175,8 @@ class DQ_CoppeliaSimInterface void set_engine(const ENGINE& engine); std::string get_engine(); + + void set_gravity(const DQ& gravity=-9.81*k_); DQ get_gravity() const; @@ -228,26 +184,66 @@ class DQ_CoppeliaSimInterface void save_scene(const std::string& path_to_filename) const; void close_scene() const; - bool load_model(const std::string& path_to_filename, - const std::string& desired_model_name, - const bool& load_model_only_if_missing = true, - const bool& remove_child_script = true); - bool load_from_model_browser(const std::string& path_to_filename, - const std::string& desired_model_name, - const bool& load_model_only_if_missing = true, - const bool& remove_child_script = true); + std::vector get_velocity_const_params() const; - void remove_child_script_from_object(const std::string& objectname, const std::string& script_name = "/Script"); + std::string get_resources_path() const; - bool object_exist_on_scene(const std::string& objectname) const; + + MatrixXd get_transformation_matrix(const std::vector& coeff_vector) const; + MatrixXd get_rotation_matrix(const DQ& r) const; + + DQ get_pose_from_direction(const DQ& direction, const DQ& point = DQ(1)); + + void set_static_object_properties(const std::string& name, + const std::string& parent_name, + const DQ& pose, + const std::vector& rgba_color); + + + void set_static_object_properties(const int& handle, + const int& parent_handle, + const DQ& pose, + const std::vector& rgba_color) const; + + void create_reference_frame(const std::string& name, + const double& scale = 1, + const std::vector& thickness_and_length = {0.005, 0.1}) const; + + void create_plane(const std::string& name, + const std::vector& sizes = {0.2,0.2}, + const std::vector& rgba_color = {1,0,0,0.5}, + const bool& add_normal = true, + const double& normal_scale = 1) const; + + void create_line(const std::string& name, + const std::vector& thickness_and_length = {0.01,1.5}, + const std::vector& rgba_color = {1,0,0,0.5}, + const bool& add_arrow = true, + const double& arrow_scale = 1) const; + + void create_cylinder(const std::string& name, + const std::vector& width_and_length = {0.2,1.0}, + const std::vector& rgba_color = {1,0,0,0.5}, + const bool& add_line = true, + const double& line_scale = 1) const; + + void merge_shapes(const int& parent_handle) const; + + std::tuple get_center_of_mass_and_inertia_matrix(const int& handle) const; + + int get_primitive_identifier(const PRIMITIVE& primitive) const; + + void set_object_parent(const int& handle, const int& parent_handle, const bool& move_child_to_parent_pose) const; + void set_object_parent(const std::string& objectname, const std::string& parent_object_name, + const bool& move_child_to_parent_pose = true); + + void remove_child_script_from_object(const std::string& objectname, const std::string& script_name = "/Script"); void set_object_name(const int& handle, const std::string& new_object_name) const; void set_object_name(const std::string& current_object_name, const std::string& new_object_name); - //-------- Methods to be implemented on Matlab--------------- - void set_object_color(const int& handle, const std::vector& rgba_color) const; @@ -266,19 +262,12 @@ class DQ_CoppeliaSimInterface void set_object_as_static(const std::string& objectname, const bool& static_object = true); - int add_primitive(const PRIMITIVE& primitive, - const std::string& name, - const std::vector& sizes) const; - - void set_object_parent(const int& handle, const int& parent_handle, const bool& move_child_to_parent_pose) const; - void set_object_parent(const std::string& objectname, const std::string& parent_object_name, - const bool& move_child_to_parent_pose = true); bool check_collision(const int& handle1, const int& handle2) const; - bool check_collision(const std::string& objectname1, const std::string& objectname2); std::tuple check_distance(const int& handle1, const int& handle2, const double& threshold = 0) const; std::tuple check_distance(const std::string& objectname1, const std::string& objectname2, const double& threshold = 0); + double compute_distance(const int& handle1, const int& handle2, const double& threshold = 0) const; @@ -286,63 +275,16 @@ class DQ_CoppeliaSimInterface const std::string& objectname2, const double& threshold = 0); - void plot_plane(const std::string& name, - const DQ& normal_to_the_plane, - const DQ& location, - const std::vector& sizes = {0.2,0.2}, - const std::vector& rgba_color = {1,0,0,0.5}, - const bool& add_normal = true, - const double& normal_scale = 1); - - void plot_line(const std::string& name, - const DQ& line_direction, - const DQ& location, - const std::vector& thickness_and_length = {0.01,1.5}, - const std::vector& rgba_color = {1,0,0,0.5}, - const bool& add_arrow = true, - const double& arrow_scale = 1); - - void plot_cylinder(const std::string& name, - const DQ& direction, - const DQ& location, - const std::vector& width_and_length = {0.2,1.0}, - const std::vector& rgba_color = {1,0,0,0.5}, - const bool& add_line = true, - const double& line_scale = 1); - - void plot_sphere(const std::string& name, - const DQ& location, - const double& size = 0.2, - const std::vector rgba_color = {1,0,0,0.5}); - - void plot_reference_frame(const std::string& name, - const DQ& pose, - const double& scale = 1, - const std::vector& thickness_and_length = {0.005, 0.1}); - - //void remove_plotted_object(const std::string& name); - void draw_permanent_trajectory(const DQ& point, - const double& size = 2, - const std::vector& color = {1,0,0}, - const int& max_item_count = 1000); + const double& size = 2, + const std::vector& color = {1,0,0}, + const int& max_item_count = 1000); int add_simulation_lua_script(const std::string& script_name, const std::string& script_code); - - void draw_trajectory(const std::string& objectname, - const double& size = 2, - const std::vector& rgb_color = {1,0,1}, - const int& max_item_count = 1000); - - void remove_object(const std::string& objectname, - const bool& remove_children = false); - std::vector get_bounding_box_size(const int& handle) const; - std::vector get_bounding_box_size(const std::string& objectname); - //----------------------------------------------------------------------- @@ -375,93 +317,83 @@ class DQ_CoppeliaSimInterface void set_mujoco_joint_stiffness(const std::string& jointname, const double& stiffness); - void set_mujoco_joint_stiffnesses(const std::vector& jointnames, - const double& stiffness); + void set_mujoco_joint_damping (const std::string& jointname, const double& damping); + void set_mujoco_joint_armature (const std::string& jointname, const double& armature); + void set_mujoco_body_friction (const std::string& bodyname, const std::vector& friction); - void set_mujoco_joint_damping(const std::string& jointname, const double& damping); - void set_mujoco_joint_dampings(const std::vector& jointnames, - const double& damping); + void pause_simulation() const; + double get_simulation_time() const; - void set_mujoco_joint_armature(const std::string& jointname, const double& armature); - void set_mujoco_joint_armatures(const std::vector& jointnames, - const double& armature); + bool is_simulation_running() const; + int get_simulation_state() const; + void set_status_bar_message(const std::string& message) const; - void set_mujoco_body_friction(const std::string& bodyname, const std::vector& friction); - void set_mujoco_body_frictions(const std::vector& bodynames, - const std::vector& friction); + std::vector get_jointnames_from_parent_object(const std::string& parent_objectname); + std::vector get_shapenames_from_parent_object(const std::string& parent_objectname, + const SHAPE_TYPE& shape_type = SHAPE_TYPE::ANY); - //---------------------------------------------------------------------------------------- - //---------------------------------------------------------------------------------------- + VectorXd get_angular_and_linear_velocities(const int& handle, + const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME) const; - std::unordered_map get_map(); //For debug - void show_map(); // For debug - void show_created_handle_map(); // For debug + VectorXd get_angular_and_linear_velocities(std::string& objectname, + const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME); - //-----------Deprecated methods---------------------------// - [[deprecated("This method is not required with ZeroMQ remote API.")]] - void disconnect(); - [[deprecated("This method is not required with ZeroMQ remote API.")]] - void disconnect_all(); - [[deprecated("The synchronous mode is now called stepping mode. Consider using set_stepping_mode(flag) instead.")]] - void set_synchronous(const bool& flag); - [[deprecated("This method is not required with ZeroMQ remote API.")]] - int wait_for_simulation_step_to_end(); - //---------------------------------------------------------// + void set_angular_and_linear_velocities(const int& handle, + const DQ& w, + const DQ& p_dot, + const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME) const; + void set_angular_and_linear_velocities(std::string& objectname, + const DQ& w, + const DQ& p_dot, + const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME); + void set_twist(const int& handle, + const DQ& twist, + const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME) const; + void set_twist(const std::string& objectname, + const DQ& twist, + const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME); - int get_primitive_identifier(const PRIMITIVE& primitive) const; + DQ get_twist(const int& handle, + const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME) const; + DQ get_twist(const std::string& objectname, + const REFERENCE& reference = REFERENCE::ABSOLUTE_FRAME); + + double get_mass(const int& handle) const; + double get_mass(const std::string& object_name); + + DQ get_center_of_mass(const int& handle, const REFERENCE& reference_frame=REFERENCE::ABSOLUTE_FRAME) const; + DQ get_center_of_mass(const std::string& object_name, const REFERENCE& reference_frame=REFERENCE::ABSOLUTE_FRAME); + + MatrixXd get_inertia_matrix(const int& handle, const REFERENCE& reference_frame=REFERENCE::BODY_FRAME); + MatrixXd get_inertia_matrix(const std::string& link_name, const REFERENCE& reference_frame=REFERENCE::BODY_FRAME); protected: - enum class AXIS{i,j,k}; - - - std::string host_{"localhost"}; - int rpcPort_{23000}; - int cntPort_{-1}; - int verbose_{-1}; - -private: - enum class UPDATE_MAP{ADD, REMOVE}; - - std::atomic client_created_{false}; - bool enable_deprecated_name_compatibility_{true}; - void _check_client() const; - [[noreturn]] void _throw_runtime_error(const std::string& msg) const; - - int MAX_TIME_IN_MILLISECONDS_TO_TRY_CONNECTION_{300}; - double elapsed_time_ {0}; - std::thread chronometer_thread_; - void _join_if_joinable_chronometer_thread(); - void _start_chronometer(); - void _check_connection(); - //-------------------map zone-------------------------------------------- - std::string _map_simulation_state(const int& state); - std::unordered_map handles_map_; - void _update_map(const std::string& objectname, const int& handle, const UPDATE_MAP& mode = UPDATE_MAP::ADD); - int _get_handle_from_map(const std::string& objectname); - - std::unordered_map> created_handles_map_; - void _update_created_handles_map(const std::string& base_objectname, - const std::vector& children_objectnames, - const UPDATE_MAP& mode = UPDATE_MAP::ADD); - - - void _removed_handles_from_handles_map(); - //------------------------------------------------------------------------ - std::string _remove_first_slash_from_string(const std::string& str) const; - bool _start_with_slash(const std::string& str) const; - std::string _get_standard_name(const std::string& str) const; + + std::vector _create_static_axis_at_origin(const int& parent_handle, + const std::string& parent_name, + const std::vector& sizes, + const AXIS& axis, + const double& alpha_color) const; ENGINE _get_engine(); + + bool _load_model(const std::string& path_to_filename, + const std::string& desired_model_name, + const bool& remove_child_script); + + void __set_status_bar_message(const std::string &message, const int& verbosity_type) const; + std::unordered_map engines_ = {{ENGINE::BULLET, 0}, {ENGINE::ODE, 1}, {ENGINE::VORTEX, 2}, {ENGINE::NEWTON, 3}, {ENGINE::MUJOCO, 4}}; + std::unordered_map engines_invmap = {{0,ENGINE::BULLET}, - {1,ENGINE::ODE}, - {2,ENGINE::VORTEX}, - {3,ENGINE::NEWTON}, - {4,ENGINE::MUJOCO}}; + {1,ENGINE::ODE}, + {2,ENGINE::VORTEX}, + {3,ENGINE::NEWTON}, + {4,ENGINE::MUJOCO}}; std::unordered_map simulation_status_ = {{0, "simulation stopped"}, @@ -473,82 +405,11 @@ class DQ_CoppeliaSimInterface {20, "simulation advancing first after pause"}, {21, "simulation advancing about to stop"}}; - std::vector _get_velocity_const_params() const; - - - bool _load_model(const std::string& path_to_filename, - const std::string& desired_model_name, - const bool& remove_child_script); - - MatrixXd _get_transformation_matrix(const std::vector& coeff_vector) const; - MatrixXd _get_rotation_matrix(const DQ& r) const; - DQ _get_pose_from_direction(const DQ& direction, const DQ& point = DQ(1)); - [[nodiscard("The created primitives must be added to the created_handles_map")]] - std::vector _create_static_axis_at_origin(const int& parent_handle, - const std::string& parent_name, - const std::vector& sizes, - const AXIS& axis, - const double& alpha_color = 1) const; - - void _set_static_object_properties(const std::string& name, - const std::string& parent_name, - const DQ& pose, - const std::vector& rgba_color); - - - void _set_static_object_properties(const int& handle, - const int& parent_handle, - const DQ& pose, - const std::vector& rgba_color) const; - - void _create_reference_frame(const std::string& name, - const double& scale = 1, - const std::vector& thickness_and_length = {0.005, 0.1}) const; - - void _create_plane(const std::string& name, - const std::vector& sizes = {0.2,0.2}, - const std::vector& rgba_color = {1,0,0,0.5}, - const bool& add_normal = true, - const double& normal_scale = 1) const; - - void _create_line(const std::string& name, - const std::vector& thickness_and_length = {0.01,1.5}, - const std::vector& rgba_color = {1,0,0,0.5}, - const bool& add_arrow = true, - const double& arrow_scale = 1) const; - - void _create_cylinder(const std::string& name, - const std::vector& width_and_length = {0.2,1.0}, - const std::vector& rgba_color = {1,0,0,0.5}, - const bool& add_line = true, - const double& line_scale = 1) const; - - void _merge_shapes(const int& parent_handle) const; +}; - std::tuple _get_center_of_mass_and_inertia_matrix(const int& handle) const; - /* For C++20 - * --------------------------------------------------- - void _check_sizes(const auto &v1, - const auto &v2, - const std::string& error_message) const - { - if (static_cast(v1.size()) != static_cast(v2.size())) - throw std::runtime_error(error_message); - }*/ - - template - void _check_sizes(const T &v1, - const U &v2, - const std::string& error_message) const - { - if (static_cast(v1.size()) != static_cast(v2.size())) - throw std::runtime_error(error_message); - } - //--------------------------------------------------- -}; diff --git a/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimModels.h b/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimModels.h deleted file mode 100644 index 27ee4b8..0000000 --- a/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimModels.h +++ /dev/null @@ -1,94 +0,0 @@ -/** -(C) Copyright 2024 DQ Robotics Developers - -This file is based on 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 . - -Contributors: -- Juan Jose Quiroz Omana - - Responsible for the original implementation. - -*/ -#pragma once -#include -#include -#include - -namespace DQ_robotics -{ -class DQ_CoppeliaSimModels -{ -public: - enum class COMPONENTS - { - REFERENCE_FRAME, - ARUCO_MARKER, - - PIONEER_P3DX, - - GPS, - - FRANKA_EMIKA_PANDA, - JACO, - UR3, - UR5, - UR10, - }; - -protected: - std::shared_ptr coppeliasim_interface_sptr_; - std::shared_ptr _get_interface_sptr() const; - std::string _get_string_from_others(const COMPONENTS& model); - - void _load_model(const COMPONENTS& model, - const std::string& desired_model_name, - const bool& load_model_only_if_missing = true, - const bool& remove_child_script = true); -public: - DQ_CoppeliaSimModels()=delete; - DQ_CoppeliaSimModels(const std::shared_ptr& coppeliasim_interface_sptr); - - void load_model(const COMPONENTS& model, - const std::string& desired_model_name, - const DQ& pose = DQ(0), - const bool& load_model_only_if_missing = true, - const bool& remove_child_script = true); - - void load_reference_frame(const std::string& desired_model_name, - const DQ& pose = DQ(0), - const bool& load_model_only_if_missing = true, - const bool& remove_child_script = true); - - void load_reference_frames(const std::vector& desired_model_names, - const std::vector& poses); - - void load_reference_frames(const std::vector& desired_model_names); - - void load_panda(const std::string& desired_model_name, - const DQ& pose = DQ(0)); - - void load_primitive(const DQ_CoppeliaSimInterface::PRIMITIVE& primitive, - const std::string& name = "shape", - const DQ& pose = DQ(1), - const std::vector sizes = {0.2,0.2,0.2}, - const std::vector rgba_color = {1,0,0,1}, - const bool& set_as_static = true, - const bool& set_as_respondable = false - ); - - -}; - -} diff --git a/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.h b/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.h index db6c672..922576f 100644 --- a/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.h +++ b/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.h @@ -1,7 +1,7 @@ /** (C) Copyright 2024 DQ Robotics Developers -This file is based on DQ Robotics. +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 @@ -16,19 +16,23 @@ This file is based on DQ Robotics. 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: -- Juan Jose Quiroz Omana - - Responsible for the original implementation. - The DQ_CoppeliaSimRobot class is partially based on the DQ_VrepRobot class - (https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_VrepRobot.h) + 1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) + - Responsible for the original implementation. This class is based on + https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_SerialVrepRobot.h */ #pragma once -#include -#include -#include -#include +#include +#include +#include +#include + + +using namespace Eigen; namespace DQ_robotics { @@ -36,16 +40,26 @@ class DQ_CoppeliaSimRobot { protected: std::string robot_name_; - std::shared_ptr coppeliasim_interface_sptr_; - - std::shared_ptr _get_interface_sptr() const; - - DQ_CoppeliaSimRobot(const std::string& robot_name, - const std::shared_ptr& coppeliasim_interface_sptr); + std::vector jointnames_; + std::string base_frame_name_; + DQ_CoppeliaSimRobot(const std::string& robot_name); public: virtual ~DQ_CoppeliaSimRobot() = default; + + virtual std::vector get_joint_names() = 0; + + virtual void set_configuration_space(const VectorXd& q) = 0; + virtual VectorXd get_configuration_space() = 0; + + virtual void set_target_configuration_space(const VectorXd& q_target)=0; + + virtual VectorXd get_configuration_space_velocities()=0; + virtual void set_target_configuration_space_velocities(const VectorXd& v_target)=0; + + virtual void set_configuration_space_torques(const VectorXd& t)=0; + virtual VectorXd get_configuration_space_torques()=0; + }; } - diff --git a/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.h b/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.h new file mode 100644 index 0000000..180d44f --- /dev/null +++ b/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.h @@ -0,0 +1,96 @@ +/** +(C) Copyright 2024 DQ Robotics Developers + +This file is based on 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 . + +Contributors: +- Juan Jose Quiroz Omana + - Responsible for the original implementation. + The DQ_CoppeliaSimRobotZMQ class is partially based on the DQ_SerialVrepRobot class + (https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_SerialVrepRobot.h) + +*/ + +#pragma once +#include +#include +#include + +namespace DQ_robotics +{ +class DQ_CoppeliaSimRobotZMQ: public DQ_CoppeliaSimRobot +{ +private: + std::shared_ptr interface_sptr_; + std::shared_ptr coppeliasim_interface_sptr_; + DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE joint_control_mode_; + bool robot_is_used_as_visualization_tool_; + + std::shared_ptr _get_exp_interface_sptr(); + + void _initialize_jointnames_from_coppeliasim(); + +protected: + std::shared_ptr _get_interface_sptr(); + + DQ_CoppeliaSimRobotZMQ(const std::string& robot_name, + const std::shared_ptr& interface_sptr); +private: + void _set_operation_modes(const DQ_CoppeliaSimInterfaceZMQ::JOINT_MODE& joint_mode, + const DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE& joint_control_mode); + void _set_robot_as_visualization_tool(); + void _set_robot_as_dynamic_tool(const DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE& joint_control_mode); + void _set_joint_control_type(const DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE& joint_control_mode); + void _set_control_inputs(const VectorXd& u); + +public: + + std::vector get_joint_names() override; + + void set_configuration_space(const VectorXd& q) override; + VectorXd get_configuration_space() override; + + void set_target_configuration_space(const VectorXd& q_target) override; + + VectorXd get_configuration_space_velocities() override; + void set_target_configuration_space_velocities(const VectorXd& v_target) override; + + void set_configuration_space_torques(const VectorXd& t) override; + VectorXd get_configuration_space_torques() override; + + //For backwards compatibility, to be removed in a future version of dqrobotics + [[deprecated("Use set_configuration_space instead")]] + void send_q_to_vrep(const VectorXd& q); + [[deprecated("Use get_configuration_space instead")]] + VectorXd get_q_from_vrep(); + + [[deprecated("Use set_configuration_space instead")]] + void set_configuration_space_positions(const VectorXd& q); + + [[deprecated("Use get_configuration_space instead")]] + VectorXd get_configuration_space_positions(); + + [[deprecated("Use set_target_configuration_space instead")]] + void set_target_configuration_space_positions(const VectorXd& q_target); + + + + +}; + +} + + diff --git a/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimZmqRobot.h b/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimZmqRobot.h new file mode 100644 index 0000000..fbed33f --- /dev/null +++ b/include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimZmqRobot.h @@ -0,0 +1,52 @@ +/** +(C) Copyright 2024 DQ Robotics Developers + +This file is based on 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 . + +Contributors: +- Juan Jose Quiroz Omana + - Responsible for the original implementation. + The DQ_CoppeliaSimZmqRobot class is partially based on the DQ_VrepRobot class + (https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_VrepRobot.h) + +*/ + +#pragma once +#include +#include +#include +#include + +/* +namespace DQ_robotics +{ +class DQ_CoppeliaSimZmqRobot +{ +protected: + std::string robot_name_; + std::shared_ptr coppeliasim_interface_sptr_; + + std::shared_ptr _get_interface_sptr() const; + + DQ_CoppeliaSimZmqRobot(const std::string& robot_name, + const std::shared_ptr& coppeliasim_interface_sptr); + +public: + virtual ~DQ_CoppeliaSimZmqRobot() = default; +}; +} + +*/ diff --git a/include/dqrobotics/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.h b/include/dqrobotics/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.h deleted file mode 100644 index ddc2b6e..0000000 --- a/include/dqrobotics/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.h +++ /dev/null @@ -1,71 +0,0 @@ -/** -(C) Copyright 2024 DQ Robotics Developers - -This file is based on 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 . - -Contributors: -- Juan Jose Quiroz Omana - - Responsible for the original implementation. - The DQ_SerialCoppeliaSimRobot class is partially based on the DQ_SerialVrepRobot class - (https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_SerialVrepRobot.h) - -*/ - -#pragma once -#include -#include - -namespace DQ_robotics -{ -class DQ_SerialCoppeliaSimRobot: public DQ_CoppeliaSimRobot -{ -protected: - std::vector jointnames_; - std::string base_frame_name_; - DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE joint_control_mode_; - bool robot_is_used_as_visualization_tool_; - - void _initialize_jointnames_from_coppeliasim(); - - DQ_SerialCoppeliaSimRobot(const std::string& robot_name, - const std::shared_ptr& coppeliasim_interface_sptr); -public: - - virtual void set_operation_modes(const DQ_CoppeliaSimInterface::JOINT_MODE& joint_mode, - const DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE& joint_control_mode); - virtual void set_robot_as_visualization_tool(); - virtual void set_robot_as_dynamic_tool(const DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE& joint_control_mode); - virtual void set_joint_control_type(const DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE& joint_control_mode); - - virtual void set_control_inputs(const VectorXd& u); - - virtual std::vector get_joint_names(); - - virtual void set_configuration_space_positions(const VectorXd& q); - virtual VectorXd get_configuration_space_positions(); - virtual void set_target_configuration_space_positions(const VectorXd& q_target); - - virtual VectorXd get_configuration_space_velocities(); - virtual void set_target_configuration_space_velocities(const VectorXd& v_target); - - virtual void set_configuration_space_torques(const VectorXd& torques); - virtual VectorXd get_configuration_space_torques(); - -}; - -} - - diff --git a/include/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimRobot.h b/include/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.h similarity index 79% rename from include/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimRobot.h rename to include/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.h index 13d6b30..b23ed08 100644 --- a/include/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimRobot.h +++ b/include/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.h @@ -26,16 +26,16 @@ This file is based on DQ Robotics. #pragma once #include -#include +#include #include namespace DQ_robotics { -class FrankaEmikaPandaCoppeliaSimRobot: public DQ_SerialCoppeliaSimRobot +class FrankaEmikaPandaCoppeliaSimZMQRobot: public DQ_CoppeliaSimRobotZMQ { public: - FrankaEmikaPandaCoppeliaSimRobot(const std::string& robot_name, - const std::shared_ptr& coppeliasim_interface_sptr + FrankaEmikaPandaCoppeliaSimZMQRobot(const std::string& robot_name, + const std::shared_ptr& coppeliasim_interface_sptr ); DQ_SerialManipulatorMDH kinematics(); }; diff --git a/include/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimRobot.h b/include/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.h similarity index 80% rename from include/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimRobot.h rename to include/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.h index 86c2bcf..e5f1b4f 100644 --- a/include/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimRobot.h +++ b/include/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.h @@ -22,27 +22,28 @@ This file is based on DQ Robotics. */ #pragma once -#include +#include #include namespace DQ_robotics { -class URXCoppeliaSimRobot: public DQ_SerialCoppeliaSimRobot -{ +class URXCoppeliaSimZMQRobot: public DQ_CoppeliaSimRobotZMQ +{ public: enum class MODEL{ UR5 }; -protected: - URXCoppeliaSimRobot::MODEL model_; public: - URXCoppeliaSimRobot(const std::string& robot_name, - const std::shared_ptr& coppeliasim_interface_sptr, + URXCoppeliaSimZMQRobot(const std::string& robot_name, + const std::shared_ptr& coppeliasim_interface_sptr, const MODEL& model = MODEL::UR5); DQ_SerialManipulatorDH kinematics(); +private: + URXCoppeliaSimZMQRobot::MODEL model_; + }; } diff --git a/matlab/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m b/matlab/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m deleted file mode 100644 index 2a4da17..0000000 --- a/matlab/interfaces/coppeliasim/DQ_CoppeliaSimInterface.m +++ /dev/null @@ -1,1013 +0,0 @@ -classdef DQ_CoppeliaSimInterface < handle - - properties (Access = private) - enable_deprecated_name_compatibility_ = true; - handles_map_; - client_created_; - client_; - sim_; - end - properties - - - end - - methods (Access = private) - - function create_client_(obj, host, rpcPort, cntPort, verbose_) - arguments - obj - host string - rpcPort uint32 - cntPort uint32 - verbose_ logical - end - - if obj.client_created_ == false - obj.client_ = RemoteAPIClient('host', host,'port',rpcPort, 'cntPort', cntPort, 'verbose', verbose_); - obj.sim_ = obj.client_.require('sim'); - end - end - - function set_status_bar_message_(obj, message, verbosity_type) - obj.sim_.addLog(verbosity_type, message); - end - - function update_map_(obj, objectname, handle) - obj.handles_map_ = insert(obj.handles_map_, objectname, handle); - end - - function rtn = get_handle_from_map_(obj, objectname) - if isConfigured(obj.handles_map_) - if (isKey(obj.handles_map_ , objectname)) - rtn = obj.handles_map_(objectname); - else - rtn = obj.get_object_handle(objectname); - end - else - rtn = obj.get_object_handle(objectname); - end - end - - function rtn = start_with_slash_(~, objectname) - k = strfind(objectname,'/'); - n = length(k); - if (n==0) - rtn = false; - else - if (k(1)==1) - rtn = true; - else - rtn = false; - end - end - end - - function newstr = remove_first_slash_from_string_(obj, str) - if obj.start_with_slash_(str) - newstr = erase(str,"/"); - else - newstr = str; - end - end - - function standard_str = get_standard_name_(obj, str) - standard_str = str; - if (~obj.start_with_slash_(str) && obj.enable_deprecated_name_compatibility_ == true) - standard_str = '/'+str; - end - end - - function check_sizes_(~, v1, v2, message) - if (length(v1) ~= length(v2)) - error(message); - end - end - - function params = get_velocity_const_params_(obj) - params = [obj.sim_.shapefloatparam_init_velocity_a; - obj.sim_.shapefloatparam_init_velocity_b; - obj.sim_.shapefloatparam_init_velocity_g; - obj.sim_.shapefloatparam_init_velocity_x; - obj.sim_.shapefloatparam_init_velocity_y; - obj.sim_.shapefloatparam_init_velocity_z]; - end - - function status = load_model_(obj, path_to_filename, desired_model_name, remove_child_script) - rtn = obj.sim_.loadModel(path_to_filename); - if (rtn ~= -1) - obj.set_object_name(obj.get_object_name(rtn), obj.remove_first_slash_from_string_(desired_model_name)); - if (remove_child_script) - obj.remove_child_script_from_object("/" +obj.remove_first_slash_from_string_(desired_model_name)); - end - status = true; - else - status = false; - end - - end - - function [COM_body_frame, Inertia_maxtrix_body_frame] = get_center_of_mass_and_inertia_matrix_(obj, objectname) - [inertia_matrix_coeff, center_of_mass_coeff] = obj.sim_.getShapeInertia(obj.get_handle_from_map_(objectname)); - - Inertia_maxtrix_body_frame = reshape([inertia_matrix_coeff{:}],[3,3]); - COM_body_frame_matrix= [center_of_mass_coeff{4}, center_of_mass_coeff{8}, center_of_mass_coeff{12}]; - - COM_body_frame = DQ(COM_body_frame_matrix); - mass = obj.get_mass(objectname); - Inertia_maxtrix_body_frame = Inertia_maxtrix_body_frame/mass; - end - - function R = get_rotation_matrix_(~, r) - vecr = r.vec4(); - w = vecr(1); - a = vecr(2); - b = vecr(3); - c = vecr(4); - R = [1-2*(b*b +c*c), 2*(a*b-w*c), 2*(a*c+w*b); - 2*(a*b+w*c), 1-2*(a*a+c*c), 2*(b*c-w*a); - 2*(a*c-w*b), 2*(b*c+w*a), 1-2*(a*a+b*b)]; - end - - function throw_runtime_error_(obj, ME, msg) - obj.stop_simulation(); - disp(msg); - rethrow(ME); %ME = MException('sayHello:inputError','Input must be char.'); - end - - - function check_client_(obj) - if (~obj.client_created_) - throw(MException('DQ_CoppeliaSimInterface:UnestablishedConnection', 'Unestablished connection. Did you use connect()?')); - end - end - - - end - - methods - - function obj = DQ_CoppeliaSimInterface() - obj.client_created_ = false; - obj.handles_map_ = dictionary; - end - function rtn = connect(obj, host, rpcPort, cntPort, verbose_) - - switch nargin - case 1 - host = 'localhost'; - rpcPort = 23000; - cntPort = -1; - verbose_ = false; - case 2 - rpcPort = 23000; - cntPort = -1; - verbose_ = false; - case 3 - cntPort = -1; - verbose_ = false; - case 4 - verbose_ = false; - case 5 - - otherwise - error('Wrong number of arguments'); - end - - obj.create_client_(host, rpcPort, cntPort, verbose_); - obj.client_created_ = true; - rtn = true; - obj.set_status_bar_message(" "); - obj.set_status_bar_message_("DQ_CoppeliaSimInterface is brought " + ... - "to you by Juan Jose Quiroz Omana", ... - obj.sim_.verbosity_warnings); - end - - function start_simulation(obj) - % This method starts the CoppeliaSim simulation. - obj.check_client_(); - obj.sim_.startSimulation(); - end - - function pause_simulation(obj) - % This method pauses the CoppeliaSim simulation. - obj.check_client_(); - obj.sim_.pauseSimulation(); - end - - function stop_simulation(obj) - % This method stops the CoppeliaSim simulation. - obj.check_client_(); - obj.sim_.stopSimulation() - end - - function set_stepping_mode(obj, flag) - % This method enables or disables the stepping mode - % (formerly known as synchronous mode). - obj.check_client_(); - obj.sim_.setStepping(flag); - end - - function simulation_time = get_simulation_time(obj) - % This method returns the simulation time. - obj.check_client_(); - simulation_time =obj.sim_.getSimulationTime(); - end - - function trigger_next_simulation_step(obj) - % This method sends a trigger signal to the CoppeliaSim scene, - % which performs a simulation step when the stepping mode is used. - obj.check_client_(); - obj.sim_.step(); - end - - function rtn = is_simulation_running(obj) - % checks if the simulation is running. Returns true if the simulation - % is running. False otherwise. - obj.check_client_(); - rtn = (obj.sim_.getSimulationState() > obj.sim_.simulation_paused); - end - - function rtn = get_simulation_state(obj) - % this method returns the simulaton state. - % See more in https://manual.coppeliarobotics.com/en/simulation.htm - % simulation_advancing = 16 - % simulation_advancing_abouttostop = 21 - % simulation_advancing_firstafterpause = 20 - % simulation_advancing_firstafterstop = 16 - % simulation_advancing_lastbeforepause = 19 - % simulation_advancing_lastbeforestop = 22 - % simulation_advancing_running = 17 - % simulation_paused = 8 - % simulation_stopped = 0 - obj.check_client_(); - rtn = obj.sim_.getSimulationState(); - end - - function set_status_bar_message(obj, message) - % This method sends a message to CoppeliaSim to be displayed - % in the status bar. - obj.check_client_(); - obj.set_status_bar_message_(message, obj.sim_.verbosity_undecorated); - end - - function handle = get_object_handle(obj, objectname) - % get_object_handle gets the object handle from CoppeliaSim. - % If the handle is not included in the map, then the map is updated. - additional_error_message = ""; - if (~obj.start_with_slash_(objectname) && ... - obj.enable_deprecated_name_compatibility_ == false) - additional_error_message = "Did you mean " + char(34) + '/' +objectname + char(34) + ' ?'; - end - - - try - obj.check_client_(); - standard_objectname = obj.get_standard_name_(objectname); - handle = obj.sim_.getObject(standard_objectname); - obj.update_map_(standard_objectname, handle); - - catch ME - - msg = "The object " + char(34) + objectname + char(34) + " does not exist in the " + ... - "current scene in CoppeliaSim. " + additional_error_message; - obj.throw_runtime_error_(ME, msg) - end - end - - function handles = get_object_handles(obj, objectnames) - % This method returns a cell that contains the handles - % corresponding to the objectnames. - arguments - obj - objectnames cell - end - n = length(objectnames); - handles = cell(1,n); - for i=1:n - handles{i} = obj.get_object_handle(objectnames{i}); - end - end - - function rtn = get_map(obj) - % For debugging - rtn = obj.handles_map_; - end - - function t = get_object_translation(obj, objectname) - arguments - obj - objectname string - end - obj.check_client_(); - position = obj.sim_.getObjectPosition(obj.get_handle_from_map_(objectname), ... - obj.sim_.handle_world); - t = DQ(double([position{1},position{2},position{3}])); - end - - function set_object_translation(obj, objectname, t) - arguments - obj - objectname string - t DQ - end - obj.check_client_(); - vec_t = vec3(t); - position = {vec_t(1), vec_t(2), vec_t(3)}; - obj.sim_.setObjectPosition(obj.get_handle_from_map_(objectname), ... - position,obj.sim_.handle_world); - end - - function r = get_object_rotation(obj, objectname) - arguments - obj - objectname string - end - obj.check_client_(); - rotation = obj.sim_.getObjectQuaternion(obj.get_handle_from_map_(objectname) ... - + obj.sim_.handleflag_wxyzquat, obj.sim_.handle_world); - - r = DQ(double([rotation{1},rotation{2},rotation{3}, rotation{4}])); - end - - function set_object_rotation(obj, objectname, r) - arguments - obj - objectname string - r DQ - end - obj.check_client_(); - vec_r = vec4(r); - rotation = {vec_r(1), vec_r(2), vec_r(3), vec_r(4)}; - obj.sim_.setObjectQuaternion(obj.get_handle_from_map_(objectname) ... - + obj.sim_.handleflag_wxyzquat, rotation, obj.sim_.handle_world); - end - - function x = get_object_pose(obj, objectname) - arguments - obj - objectname string - end - t = obj.get_object_translation(objectname); - r = obj.get_object_rotation(objectname); - x = r + 0.5*DQ.E*t*r; - end - - function set_object_pose(obj, objectname, h) - arguments - obj - objectname string - h DQ - end - obj.check_client_(); - vec_r = h.P().vec4(); - vec_t = h.translation().vec3(); - pose = {vec_t(1), vec_t(2), vec_t(3),vec_r(1), vec_r(2), vec_r(3), vec_r(4)}; - obj.sim_.setObjectPose(obj.get_handle_from_map_(objectname) + obj.sim_.handleflag_wxyzquat, ... - pose, obj.sim_.handle_world); - end - - function theta = get_joint_position(obj, jointname) - arguments - obj - jointname string - end - obj.check_client_(); - theta = double(obj.sim_.getJointPosition(obj.get_handle_from_map_(jointname))); - - end - - function q = get_joint_positions(obj, jointnames) - arguments - obj - jointnames cell - end - n = length(jointnames); - q = zeros(n,1); - for i=1:n - q(i) = obj.get_joint_position(jointnames{i}); - end - end - - function set_joint_position(obj, jointname, angle_rad) - arguments - obj - jointname string - angle_rad double - end - obj.check_client_(); - obj.sim_.setJointPosition(obj.get_handle_from_map_(jointname), angle_rad); - end - - function set_joint_positions(obj, jointnames, angles_rad) - arguments - obj - jointnames cell - angles_rad double - end - message = "Error in DQ_CoppeliaSimInterface.set_joint_positions: " + ... - "jointnames and angles_rad have incompatible sizes"; - obj.check_sizes_(jointnames, angles_rad, message); - n = length(jointnames); - for i=1:n - obj.set_joint_position(jointnames{i}, angles_rad(i)); - end - end - - function set_joint_target_position(obj, jointname, angle_rad) - arguments - obj - jointname string - angle_rad double - end - obj.check_client_(); - obj.sim_.setJointTargetPosition(obj.get_handle_from_map_(jointname), angle_rad); - end - - function set_joint_target_positions(obj, jointnames, angles_rad) - arguments - obj - jointnames cell - angles_rad double - end - message = "Error in DQ_CoppeliaSimInterface.set_joint_target_positions: " + ... - "jointnames and angles_rad have incompatible sizes"; - obj.check_sizes_(jointnames, angles_rad, message); - n = length(jointnames); - for i=1:n - obj.set_joint_target_position(jointnames{i}, angles_rad(i)); - end - end - - function theta_dot = get_joint_velocity(obj, jointname) - arguments - obj - jointname string - end - obj.check_client_(); - theta_dot = obj.sim_.getObjectFloatParam(obj.get_handle_from_map_(jointname), ... - obj.sim_.jointfloatparam_velocity); - end - - function q_dot = get_joint_velocities(obj, jointnames) - arguments - obj - jointnames cell - end - n = length(jointnames); - q_dot = zeros(n,1); - for i=1:n - q_dot(i) = obj.get_joint_velocity(jointnames{i}); - end - end - - function set_joint_target_velocity(obj, jointname, angle_rad_dot) - arguments - obj - jointname string - angle_rad_dot double - end - obj.check_client_(); - obj.sim_.setJointTargetVelocity(obj.get_handle_from_map_(jointname), angle_rad_dot); - end - - function set_joint_target_velocities(obj, jointnames, angles_rad_dot) - arguments - obj - jointnames cell - angles_rad_dot double - end - message = "Error in DQ_CoppeliaSimInterface.set_joint_target_velocities: " + ... - "jointnames and angles_rad_dot have incompatible sizes"; - obj.check_sizes_(jointnames, angles_rad_dot, message); - n = length(jointnames); - for i=1:n - obj.set_joint_target_velocity(jointnames{i}, angles_rad_dot(i)); - end - end - - function set_joint_torque(obj, jointname, torque) - arguments - obj - jointname string - torque double - end - angle_dot_rad_max = 10000.0; - if (torque==0) - angle_dot_rad_max = 0.0; - elseif (torque<0) - angle_dot_rad_max = -10000.0; - end - obj.check_client_(); - handle = obj.get_handle_from_map_(jointname); - obj.sim_.setJointTargetVelocity(handle, angle_dot_rad_max); - obj.sim_.setJointTargetForce(handle, abs(torque)); - end - - function set_joint_torques(obj, jointnames, torques) - arguments - obj - jointnames cell - torques double - end - message = "Error in DQ_CoppeliaSimInterface.set_joint_torques: " + ... - "jointnames and torques have incompatible sizes"; - obj.check_sizes_(jointnames, torques, message); - n = length(jointnames); - for i=1:n - obj.set_joint_torque(jointnames{i}, torques(i)); - end - end - - function torque = get_joint_torque(obj, jointname) - arguments - obj - jointname string - end - - try - torque = obj.sim_.getJointForce(obj.get_handle_from_map_(jointname)); - catch - disp("This method has a bug in CoppeliaSim 4.6.0-rev18. " + ... - "https://forum.coppeliarobotics.com/viewtopic.php?p=40811#p40811"); - disp("Meanwhile, to avoid this error, don't read the force in the first simulation step.") - end - end - - function torques = get_joint_torques(obj, jointnames) - arguments - obj - jointnames cell - end - - n = length(jointnames); - torques = zeros(n,1); - for i=1:n - torques(i) = obj.get_joint_torque(jointnames{i}); - end - end - - function objectname= get_object_name(obj, handle) - objectname = obj.sim_.getObjectAlias(handle, 1); - obj.update_map_(objectname, handle); - end - - function objectnames = get_object_names(obj, handles) - arguments - obj - handles cell - end - n = length(handles); - objectnames = cell(1,n); - for i=1:n - objectnames{i}=obj.get_object_name(handles{i}); - end - end - - function jointnames = get_jointnames_from_base_objectname(obj, base_objectname) - arguments - obj - base_objectname string - end - obj.check_client_(); - base_handle = obj.get_handle_from_map_(base_objectname); - jointhandles = obj.sim_.getObjectsInTree(base_handle,... - obj.sim_.object_joint_type,0); - jointnames = obj.get_object_names(jointhandles); - end - - function linknames = get_linknames_from_base_objectname(obj, base_objectname) - arguments - obj - base_objectname string - end - obj.check_client_(); - base_handle = obj.get_handle_from_map_(base_objectname); - shapehandles = obj.sim_.getObjectsInTree(base_handle,obj.sim_.object_shape_type, 0); - linknames = obj.get_object_names(shapehandles); - end - - - function v = get_angular_and_linear_velocities(obj, objectname, reference) - arguments - obj - objectname string - reference DQ_CoppeliaSimInterface_REFERENCE - end - params = obj.get_velocity_const_params_(); - n = length(params); - v = zeros(n,1); - for i=1:n - v(i) = obj.sim_.getObjectFloatParam(obj.get_handle_from_map_(objectname), params(i)); - end - if reference == DQ_CoppeliaSimInterface_REFERENCE.BODY_FRAME - x = obj.get_object_pose(objectname); - r = x.P(); - w_b = r.conj()*DQ(v(1:3))*r; - p_dot_b = r.conj()*DQ(v(4:6))*r; - v = [vec3(w_b); vec3(p_dot_b)]; - end - end - - - function set_angular_and_linear_velocities(obj, objectname, w, p_dot, reference) - arguments - obj - objectname string - w DQ - p_dot DQ - reference DQ_CoppeliaSimInterface_REFERENCE - end - params = obj.get_velocity_const_params_(); - n = length(params); - if reference == DQ_CoppeliaSimInterface_REFERENCE.ABSOLUTE_FRAME - w_a = w; - p_dot_a = p_dot; - v = [w_a.vec3(); - p_dot_a.vec3()]; - else - x = obj.get_object_pose(objectname); - r = x.P(); - w_a = r*w*r.conj(); - p_dot_a = r*p_dot*r.conj(); - v = [w_a.vec3(); - p_dot_a.vec3()]; - end - obj.check_client_(); - handle = obj.get_handle_from_map_(objectname); - obj.sim_.resetDynamicObject(handle); - for i=1:n - obj.sim_.setObjectFloatParam(handle, params(i), v(i)); - end - end - - - function twist = get_twist(obj, objectname, reference) - arguments - obj - objectname string - reference DQ_CoppeliaSimInterface_REFERENCE - end - v = obj.get_angular_and_linear_velocities(objectname, reference); - w = DQ(v(1:3)); - p_dot = DQ(v(4:6)); - x = obj.get_object_pose(objectname); - twist = w + DQ.E*(p_dot + cross(x.translation(), w)); - if (reference == DQ_CoppeliaSimInterface_REFERENCE.BODY_FRAME) - twist = x.conj()*twist*x; - end - end - - - function set_twist(obj, objectname, twist, reference) - arguments - obj - objectname string - twist DQ - reference DQ_CoppeliaSimInterface_REFERENCE - end - - if (~is_pure(twist)) - error("Bad set_object_twist() call: Not a pure dual quaternion"); - end - if (reference == DQ_CoppeliaSimInterface_REFERENCE.BODY_FRAME) - obj.set_angular_and_linear_velocities(objectname, twist.P(), twist.D(),... - DQ_CoppeliaSimInterface_REFERENCE.BODY_FRAME); - else - x = obj.get_object_pose(objectname); - obj.set_angular_and_linear_velocities(objectname, twist.P(), twist.D()-cross(x.translation(), twist.P()),... - DQ_CoppeliaSimInterface_REFERENCE.ABSOLUTE_FRAME); - end - end - - - function set_joint_mode(obj, jointname, joint_mode) - arguments - obj - jointname string - joint_mode DQ_CoppeliaSimInterface_JOINT_MODE - end - switch(joint_mode) - case DQ_CoppeliaSimInterface_JOINT_MODE.KINEMATIC - jointMode = obj.sim_.jointmode_kinematic; - case DQ_CoppeliaSimInterface_JOINT_MODE.DYNAMIC - jointMode = obj.sim_.jointmode_dynamic; - case DQ_CoppeliaSimInterface_JOINT_MODE.DEPENDENT - jointMode = obj.sim_.jointmode_dependent; - end - obj.sim_.setJointMode(obj.get_handle_from_map_(jointname), jointMode, 0); - end - - - function set_joint_modes(obj, jointnames, joint_mode) - arguments - obj - jointnames cell - joint_mode DQ_CoppeliaSimInterface_JOINT_MODE - end - for i=1:length(jointnames) - obj.set_joint_mode(jointnames{i}, joint_mode); - end - end - - - function set_joint_control_mode(obj, jointname, joint_control_mode) - arguments - obj - jointname string - joint_control_mode DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE - end - switch (joint_control_mode) - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.FREE - control_mode = obj.sim_.jointdynctrl_free; - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.FORCE - control_mode = obj.sim_.jointdynctrl_force; - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.VELOCITY - control_mode = obj.sim_.jointdynctrl_velocity; - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.POSITION - control_mode = obj.sim_.jointdynctrl_position; - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.SPRING - control_mode = obj.sim_.ointdynctrl_spring; - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.CUSTOM - control_mode = obj.sim_.jointdynctrl_callback; - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.TORQUE - control_mode = obj.sim_.jointdynctrl_velocity; - end - obj.check_client_(); - obj.sim_.setObjectInt32Param(obj.get_handle_from_map_(jointname),... - obj.sim_.jointintparam_dynctrlmode, ... - control_mode); - end - - - function set_joint_control_modes(obj, jointnames, joint_control_mode) - arguments - obj - jointnames cell - joint_control_mode DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE - end - for i=1:length(jointnames) - obj.set_joint_control_mode(jointnames{i}, joint_control_mode); - end - end - - - function enable_dynamics(obj, flag) - arguments - obj - flag logical - end - obj.check_client_(); - obj.sim_.setBoolParam(obj.sim_.boolparam_dynamics_handling_enabled, flag); - end - - function t = get_simulation_time_step(obj) - obj.check_client_(); - t = obj.sim_.getFloatParam(obj.sim_.floatparam_simulation_time_step); - end - - function set_simulation_time_step(obj, time_step) - obj.check_client_(); - obj.sim_.setFloatParam(obj.sim_.floatparam_simulation_time_step, time_step); - end - - function t = get_physics_time_step(obj) - obj.check_client_(); - t = obj.sim_.getFloatParam(obj.sim_.floatparam_physicstimestep); - end - - function set_physics_time_step(obj, time_step) - obj.check_client_(); - obj.sim_.setFloatParam(obj.sim_.floatparam_physicstimestep, time_step); - end - - function set_dynamic_engine(obj, engine) - arguments - obj - engine DQ_CoppeliaSimInterface_ENGINE - end - obj.check_client_(); - obj.sim_.setInt32Param(obj.sim_.intparam_dynamic_engine, engine); - end - - function set_gravity(obj, gravity) - arguments - obj - gravity DQ - end - gravity_vec = gravity.vec3(); - g = {gravity_vec(1),gravity_vec(2), gravity_vec(3)}; - obj.sim_.setArrayParam(obj.sim_.arrayparam_gravity, g); - end - - function gravity = get_gravity(obj) - obj.check_client_(); - g = obj.sim_.getArrayParam(obj.sim_.arrayparam_gravity); - gravity = DQ([0, g{1}, g{2}, g{3}]); - end - - - function load_scene(obj, path_to_filename) - % * @brief DQ_CoppeliaSimInterface.load_scene loads a scene from your computer. - % * @param path_to_filename the path to the scene. This string must containt - % * the file extension. - % * - % * Example: - % * - % * load_scene("/Users/juanjqo/git/space_robot/scenes/space_robot.ttt"); - arguments - obj - path_to_filename string - end - obj.check_client_(); - obj.sim_.loadScene(path_to_filename); - end - - function save_scene(obj, path_to_filename) - obj.check_client_(); - obj.sim_.saveScene(path_to_filename); - end - - function close_scene(obj) - obj.check_client_(); - obj.sim_.closeScene(); - end - - function status = load_model(obj, path_to_filename, desired_model_name, load_model_only_if_missing,remove_child_script) - % /** - % * @brief DQ_CoppeliaSimInterface::load_model loads a model to - % * the scene. - % * - % * @param path_to_filename The path to the model. - % * @param desired_model_name The name you want for the loaded model. - % * @param load_model_only_if_missing If the model exists (with the same alias) - % * the model is not loaded. (Default) - % * @param remove_child_script Remove the associated child script of the model - % * (Default) - % * @return A boolean flag. True if the model was loaded. False otherwise. - % */ - switch (nargin) - case 3 - load_model_only_if_missing = true; - remove_child_script = true; - case 4 - remove_child_script = true; - case 5 - - otherwise - error("Wrong number of parameters"); - end - if (load_model_only_if_missing == true) - if (~obj.object_exist_on_scene("/"+obj.remove_first_slash_from_string_(desired_model_name))) - status = obj.load_model_(path_to_filename, desired_model_name, remove_child_script); - else - status = true; - end - else - status = obj.load_model_(path_to_filename, desired_model_name, remove_child_script); - end - end - - - function status = load_from_model_browser(obj, path_to_filename, desired_model_name, load_model_only_if_missing, remove_child_script) - % /** - % * @brief DQ_CoppeliaSimInterface::load_from_model_browser loads a model from - % * the CoppeliaSim model browser. - % * - % * Ex: load_from_model_browser("/robots/non-mobile/FrankaEmikaPanda.ttm", - % "/Franka"); - % * @param path_to_filename The path to the model relative to the model browser. - % * @param desired_model_name The name you want for the loaded model. - % * @param load_model_only_if_missing If the model exists (with the same alias) - % * the model is not loaded. (Default) - % * @param remove_child_script Remove the associated child script of the model - % * (Default) - % * @return A boolean flag. True if the model was loaded. False otherwise. - % */ - switch (nargin) - case 3 - load_model_only_if_missing = true; - remove_child_script = true; - case 4 - remove_child_script = true; - otherwise - error("Wrong number of parameters"); - end - resources_path = obj.sim_.getStringParam(obj.sim_.stringparam_resourcesdir); - status = obj.load_model(resources_path + "/models" + path_to_filename, ... - desired_model_name, load_model_only_if_missing, remove_child_script); - end - - - function remove_child_script_from_object(obj, objectname, script_name) - - if nargin==2 - script_name = "/Script"; - end - % script_handle = obj.sim_.getScript(obj.sim_.scripttype_childscript,... - % obj.get_handle_from_map_(objectname)); - % if (script_handle ~= -1) - % obj.sim_.removeScript(script_handle); - % end - - obj.check_client_(); - if (obj.object_exist_on_scene(obj.get_standard_name_(objectname)+script_name)) - - handle = obj.get_handle_from_map_(obj.get_standard_name_(objectname)+script_name); - obj.sim_.removeObjects({handle}, false); - end - end - - - function status = object_exist_on_scene(obj, objectname) - obj.check_client_(); - options = {{"noError", false}}; - try - rtn = obj.sim_.getObject(objectname, options); - if (rtn ~= -1) - status = true; - else - status = false; - end - catch - status = false; - end - end - - function set_object_name(obj, current_object_name, new_object_name) - arguments - obj - current_object_name string - new_object_name string - end - obj.check_client_(); - obj.sim_.setObjectAlias(obj.get_handle_from_map_(current_object_name), new_object_name); - end - - function mass = get_mass(obj, objectname) - arguments - obj - objectname string - end - obj.check_client_(); - mass = obj.sim_.getShapeMassAndInertia(obj.get_handle_from_map_(objectname)); - end - - function Im = get_inertia_matrix(obj, link_name, reference_frame) - arguments - obj - link_name string - reference_frame DQ_CoppeliaSimInterface_REFERENCE - end - [COM_body_frame, Inertia_maxtrix_body_frame] = obj.get_center_of_mass_and_inertia_matrix_(link_name); - if reference_frame == DQ_CoppeliaSimInterface_REFERENCE.BODY_FRAME - Im = Inertia_maxtrix_body_frame; - else - x_0_bodyFrame = obj.get_object_pose(link_name); - x_bodyFrame_com = 1 + 0.5*DQ.E*COM_body_frame; - x_0_com = x_0_bodyFrame*x_bodyFrame_com; - R_0_COM = obj.get_rotation_matrix_(x_0_com.P()); - Im = R_0_COM*Inertia_maxtrix_body_frame*R_0_COM'; - end - - end - - function com = get_center_of_mass(obj, object_name, reference_frame) - arguments - obj - object_name string - reference_frame DQ_CoppeliaSimInterface_REFERENCE - end - [COM_body_frame, ~] = obj.get_center_of_mass_and_inertia_matrix_(object_name); - - if reference_frame == DQ_CoppeliaSimInterface_REFERENCE.BODY_FRAME - com = COM_body_frame; - else - x_0_bodyFrame = obj.get_object_pose(object_name); - x_bodyFrame_com = 1 + 0.5*DQ.E*COM_body_frame; - com = translation((x_0_bodyFrame*x_bodyFrame_com)); - end - end - - - - - %% Deprecated methods - function set_synchronous(obj, flag) - % The synchronous mode is now called stepping mode. Consider - % using set_stepping_mode(flag) instead." - obj.set_stepping_mode(flag); - end - - function disconnect(~) - end - - function disconnect_all(~) - end - - function wait_for_simulation_step_to_end(~) - end - - - end -end \ No newline at end of file diff --git a/matlab/interfaces/coppeliasim/DQ_CoppeliaSimRobot.m b/matlab/interfaces/coppeliasim/DQ_CoppeliaSimRobot.m deleted file mode 100644 index f49e842..0000000 --- a/matlab/interfaces/coppeliasim/DQ_CoppeliaSimRobot.m +++ /dev/null @@ -1,18 +0,0 @@ -classdef DQ_CoppeliaSimRobot < handle - properties (Access = protected) - robot_name_ string - coppeliasim_interface_sptr_ DQ_CoppeliaSimInterface - end - - methods (Access = protected) - function obj = DQ_CoppeliaSimRobot(robot_name, coppeliasim_interface_sptr) - obj.robot_name_ = robot_name; - obj.coppeliasim_interface_sptr_ = coppeliasim_interface_sptr; - end - - function rtn = get_interface_sptr_(obj) - rtn = obj.coppeliasim_interface_sptr_; - end - end -end - diff --git a/matlab/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.m b/matlab/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.m deleted file mode 100644 index 706bf8d..0000000 --- a/matlab/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.m +++ /dev/null @@ -1,113 +0,0 @@ -classdef DQ_SerialCoppeliaSimRobot < DQ_CoppeliaSimRobot - - properties (Access = protected) - jointnames_ cell - base_frame_name_ string - joint_control_mode_ DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE - robot_is_used_as_visualization_tool_ logical - end - - methods (Access = protected) - function obj = DQ_SerialCoppeliaSimRobot(robot_name, coppeliasim_interface_sptr) - obj = obj@DQ_CoppeliaSimRobot(robot_name, coppeliasim_interface_sptr); - obj.initialize_jointnames_from_coppeliasim_(); - % // By Default, the robot is controlled by joint positions with both the dynamic engine - % // and the stepping mode enabled. - obj.joint_control_mode_ = DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.POSITION; - obj.robot_is_used_as_visualization_tool_ = false; - end - - function initialize_jointnames_from_coppeliasim_(obj) - obj.jointnames_ = obj.get_interface_sptr_().get_jointnames_from_base_objectname(obj.robot_name_); - obj.base_frame_name_ = obj.jointnames_{1}; - end - end - - methods - function set_operation_modes(obj, joint_mode, joint_control_mode) - arguments - obj - joint_mode DQ_CoppeliaSimInterface_JOINT_MODE - joint_control_mode DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE - end - obj.get_interface_sptr_().set_joint_modes(obj.jointnames_, joint_mode); - obj.get_interface_sptr_().set_joint_control_modes(obj.jointnames_, joint_control_mode); - end - - function set_robot_as_visualization_tool(obj) - - obj.get_interface_sptr_().set_stepping_mode(false); - obj.get_interface_sptr_().enable_dynamics(false); - obj.get_interface_sptr_().set_joint_modes(obj.jointnames_,... - DQ_CoppeliaSimInterface_JOINT_MODE.KINEMATIC); - obj.robot_is_used_as_visualization_tool_ = true; - end - - function set_joint_control_type(obj, joint_control_mode) - arguments - obj - joint_control_mode DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE - end - obj.joint_control_mode_ = joint_control_mode; - obj.get_interface_sptr_().enable_dynamics(true); - obj.get_interface_sptr_().set_stepping_mode(true); - obj.set_operation_modes(DQ_CoppeliaSimInterface_JOINT_MODE.DYNAMIC, obj.joint_control_mode_); - end - - function set_control_inputs(obj, u) - if (obj.robot_is_used_as_visualization_tool_) - obj.get_interface_sptr_().set_joint_positions(obj.jointnames_, u); - else - switch (obj.joint_control_mode_) - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.FREE - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.FORCE - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.VELOCITY - obj.get_interface_sptr_().set_joint_target_velocities(obj.jointnames_, u); - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.POSITION - obj.get_interface_sptr_().set_joint_target_positions(obj.jointnames_, u); - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.SPRING - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.CUSTOM - case DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.TORQUE - obj.get_interface_sptr_().set_joint_torques(obj.jointnames_, u); - end - obj.get_interface_sptr_().trigger_next_simulation_step(); - end - - end - - function rtn = get_joint_names(obj) - rtn = obj.jointnames_; - end - - function set_configuration_space_positions(obj, q) - obj.get_interface_sptr_().set_joint_positions(obj.jointnames_,q); - end - - function q = get_configuration_space_positions(obj) - q = obj.get_interface_sptr_().get_joint_positions(obj.jointnames_); - end - - function set_target_configuration_space_positions(obj, q_target) - obj.get_interface_sptr_().set_joint_target_positions(obj.jointnames_, q_target); - end - - function q_dot = get_configuration_space_velocities(obj) - q_dot = obj.get_interface_sptr_().get_joint_velocities(obj.jointnames_); - end - - function set_target_configuration_space_velocities(obj, v_target) - obj.get_interface_sptr_().set_joint_target_velocities(obj.jointnames_, v_target); - end - - function set_configuration_space_torques(obj, torques) - obj.get_interface_sptr_().set_joint_torques(obj.jointnames_, torques); - end - - - function torques = get_configuration_space_torques(obj) - torques = obj.get_interface_sptr_().get_joint_torques(obj.jointnames_); - end - - end -end - diff --git a/matlab/interfaces/coppeliasim/enumeration_classes/DQ_CoppeliaSimInterface_ENGINE.m b/matlab/interfaces/coppeliasim/enumeration_classes/DQ_CoppeliaSimInterface_ENGINE.m deleted file mode 100644 index ad2876a..0000000 --- a/matlab/interfaces/coppeliasim/enumeration_classes/DQ_CoppeliaSimInterface_ENGINE.m +++ /dev/null @@ -1,11 +0,0 @@ -classdef DQ_CoppeliaSimInterface_ENGINE < uint32 - enumeration - BULLET (0) - ODE (1) - VORTEX (2) - NEWTON (3) - MUJOCO (4) - end -end - - diff --git a/matlab/interfaces/coppeliasim/enumeration_classes/DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.m b/matlab/interfaces/coppeliasim/enumeration_classes/DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.m deleted file mode 100644 index 27f9d81..0000000 --- a/matlab/interfaces/coppeliasim/enumeration_classes/DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE.m +++ /dev/null @@ -1,13 +0,0 @@ -classdef DQ_CoppeliaSimInterface_JOINT_CONTROL_MODE - enumeration - FREE, - FORCE, - VELOCITY, - POSITION, - SPRING, - CUSTOM, - TORQUE - end -end - - diff --git a/matlab/interfaces/coppeliasim/enumeration_classes/DQ_CoppeliaSimInterface_JOINT_MODE.m b/matlab/interfaces/coppeliasim/enumeration_classes/DQ_CoppeliaSimInterface_JOINT_MODE.m deleted file mode 100644 index e3a9e4e..0000000 --- a/matlab/interfaces/coppeliasim/enumeration_classes/DQ_CoppeliaSimInterface_JOINT_MODE.m +++ /dev/null @@ -1,7 +0,0 @@ -classdef DQ_CoppeliaSimInterface_JOINT_MODE - enumeration - KINEMATIC - DYNAMIC - DEPENDENT - end -end \ No newline at end of file diff --git a/matlab/interfaces/coppeliasim/enumeration_classes/DQ_CoppeliaSimInterface_REFERENCE.m b/matlab/interfaces/coppeliasim/enumeration_classes/DQ_CoppeliaSimInterface_REFERENCE.m deleted file mode 100644 index 33031b7..0000000 --- a/matlab/interfaces/coppeliasim/enumeration_classes/DQ_CoppeliaSimInterface_REFERENCE.m +++ /dev/null @@ -1,6 +0,0 @@ -classdef DQ_CoppeliaSimInterface_REFERENCE - enumeration - BODY_FRAME, - ABSOLUTE_FRAME - end -end \ No newline at end of file diff --git a/matlab/interfaces/coppeliasim/enumeration_classes/URXCoppeliaSimRobot_MODEL.m b/matlab/interfaces/coppeliasim/enumeration_classes/URXCoppeliaSimRobot_MODEL.m deleted file mode 100644 index 6cd24c6..0000000 --- a/matlab/interfaces/coppeliasim/enumeration_classes/URXCoppeliaSimRobot_MODEL.m +++ /dev/null @@ -1,6 +0,0 @@ -classdef URXCoppeliaSimRobot_MODEL - enumeration - UR5 - end -end - diff --git a/matlab/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimRobot.m b/matlab/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimRobot.m deleted file mode 100644 index 1d1ccf5..0000000 --- a/matlab/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimRobot.m +++ /dev/null @@ -1,15 +0,0 @@ -classdef FrankaEmikaPandaCoppeliaSimRobot < DQ_SerialCoppeliaSimRobot - - methods - function obj = FrankaEmikaPandaCoppeliaSimRobot(robot_name, coppeliasim_interface_sptr) - obj = obj@DQ_SerialCoppeliaSimRobot(robot_name, coppeliasim_interface_sptr); - end - - function kin = kinematics(obj) - kin = FrankaEmikaPandaRobot.kinematics(); - kin.set_reference_frame(obj.get_interface_sptr_().get_object_pose(obj.base_frame_name_)); - kin.set_base_frame(obj.get_interface_sptr_().get_object_pose(obj.base_frame_name_)); - end - end -end - diff --git a/matlab/interfaces/coppeliasim/robots/URXCoppeliaSimRobot.m b/matlab/interfaces/coppeliasim/robots/URXCoppeliaSimRobot.m deleted file mode 100644 index e6f120a..0000000 --- a/matlab/interfaces/coppeliasim/robots/URXCoppeliaSimRobot.m +++ /dev/null @@ -1,46 +0,0 @@ -classdef URXCoppeliaSimRobot < DQ_SerialCoppeliaSimRobot - properties (Access = private) - model_ URXCoppeliaSimRobot_MODEL - end - - methods (Access = private) - function raw_dh_matrix = get_dh_matrix_(obj, model) - arguments - obj - model URXCoppeliaSimRobot_MODEL - end - switch (model) - case URXCoppeliaSimRobot_MODEL.UR5 - raw_dh_matrix = [-pi/2, -pi/2, 0, -pi/2, 0, 0; - 0.089159-0.02315, 0, 0, 0.10915, 0.09465, 0.0823; - 0, -0.425, -0.39225, 0, 0, 0; - pi/2,0,0,pi/2,-pi/2,0; - repmat(DQ_SerialManipulatorDH.JOINT_ROTATIONAL,1,6)]; - - otherwise - raw_dh_matrix = Zero(0,0); - - end - end - - end - - methods - function obj = URXCoppeliaSimRobot(robot_name, coppeliasim_interface_sptr, model) - arguments - robot_name string - coppeliasim_interface_sptr DQ_CoppeliaSimInterface - model URXCoppeliaSimRobot_MODEL - end - obj = obj@DQ_SerialCoppeliaSimRobot(robot_name, coppeliasim_interface_sptr); - obj.model_ = model; - end - - function kin = kinematics(obj) - kin = DQ_SerialManipulatorDH(obj.get_dh_matrix_(obj.model_)); - obj.base_frame_name_ - kin.set_reference_frame(obj.get_interface_sptr_().get_object_pose(obj.base_frame_name_)); - kin.set_base_frame(obj.get_interface_sptr_().get_object_pose(obj.base_frame_name_)); - end - end -end diff --git a/matlab/interfaces/coppeliasim/tests/example.m b/matlab/interfaces/coppeliasim/tests/example.m deleted file mode 100644 index c11dad7..0000000 --- a/matlab/interfaces/coppeliasim/tests/example.m +++ /dev/null @@ -1,121 +0,0 @@ -clear all; -close all; -clc; - -include_namespace_dq - -vi = DQ_CoppeliaSimInterface(); -vi.disconnect_all(); -vi.connect(); -vi.set_synchronous(true); -vi.start_simulation(); - - -jointnames = vi.get_jointnames_from_base_objectname("/LBR4p"); - -%---------------------Robot definition--------------------------% -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(DQ_JointType.REVOLUTE,1,7)); -LBR4p_DH_matrix = [LBR4p_DH_theta; - LBR4p_DH_d; - LBR4p_DH_a; - LBR4p_DH_alpha - LBR4p_DH_type]; - -robot = DQ_SerialManipulatorDH(LBR4p_DH_matrix); -robot.set_effector(1+0.5*DQ.E*DQ.k*0.07); - -n = robot.get_dim_configuration_space(); -qmin = [-pi -pi -pi/2 -pi -pi/2 -pi -pi/2]'; -qmax = [pi pi pi/2 pi pi/2 pi pi/2]'; - -try - xbase = vi.get_object_pose(jointnames{1}); - robot.set_reference_frame(xbase); - robot.set_base_frame(xbase); - vi.set_object_pose('base', xbase); - %---------------------------------------------------------------- - %---------------------Controller definition----------------------% - qp_solver = DQ_QuadprogSolver(); - controller = DQ_ClassicQPController(robot, qp_solver); - controller.set_gain(0.2); - controller.set_damping(0.05); - controller.set_control_objective(ControlObjective.DistanceToPlane) - - xplane = vi.get_object_pose('target_plane'); - plane_d = Adsharp(xplane,-k_); - controller.set_target_primitive(plane_d); - controller.set_stability_threshold(0.00001); - desired_distance_to_target_plane = 0; - %---------------------------------------------------------------- - % Plane constraints - safe_distance = 0.1; - wall_plane = Adsharp(vi.get_object_pose('wall_plane'), k_); - floor_plane = Adsharp(vi.get_object_pose('floor_plane'), k_); - back_plane = Adsharp(vi.get_object_pose('back_plane'), k_); - planes = {wall_plane, floor_plane, back_plane}; - %---------------------------------------------------------------- - - j=1; - while ~controller.system_reached_stable_region() - %---------------------------------------------------------------% - % This command performs a simulation step in CoppeliaSim. This step - % is required when you are working in synchronous mode. - vi.trigger_next_simulation_step(); - - % Waits until the simulation step is finished. - vi.wait_for_simulation_step_to_end(); - %---------------------------------------------------------------% - - q = vi.get_joint_positions(jointnames); - x_pose = robot.fkm(q); - p = translation(x_pose); - J_pose = robot.pose_jacobian(q); - J_trans = robot.translation_jacobian(J_pose,x_pose); - - for i=1:size(planes,2) - Jdists(i,:) = robot.point_to_plane_distance_jacobian(J_trans, p, planes{i}); - dists(i) = DQ_Geometry.point_to_plane_distance(p, planes{i}) -safe_distance; - end - - A = [-Jdists; -eye(n); eye(n)]; - b = [dists'; (q-qmin); -(q-qmax)]; - controller.set_inequality_constraint(A,b); - u = controller.compute_setpoint_control_signal(q, ... - desired_distance_to_target_plane); - - %---------------------------------------------------------------% - % Set the target joint velocities in CoppeliaSim. This command is - % required when you are working in synchronous mode and the robot - % joints are set in velocity control mode. - vi.set_joint_target_velocities(jointnames, u); - %---------------------------------------------------------------% - - %---------------------------------------------------------------% - %----Set the effector frame object on CoppeliaSim to check if - %----the kinematic model is matching. This step is optional. - vi.set_object_pose('effector', robot.fkm(q)) - %---------------------------------------------------------------% - - norm(controller.get_last_error_signal()) - - - u_log(:,j) = u; - q_dot(:,j) = vi.get_joint_velocities(jointnames); - j = j+1; - end - % Before closing the connection to CoppeliaSim, - % make sure that the last command sent out had time to arrive. - vi.wait_for_simulation_step_to_end(); - vi.stop_simulation(); - vi.disconnect(); - %show_joint_velocities(u_log, q_dot); - -catch ME - vi.stop_simulation(); - vi.disconnect(); - rethrow(ME) -end \ No newline at end of file diff --git a/matlab/interfaces/coppeliasim/tests/minimal_example.m b/matlab/interfaces/coppeliasim/tests/minimal_example.m deleted file mode 100644 index e1edfb3..0000000 --- a/matlab/interfaces/coppeliasim/tests/minimal_example.m +++ /dev/null @@ -1,10 +0,0 @@ -client = RemoteAPIClient(); -sim = client.require('sim'); - -sim.startSimulation(); - -handle = sim.getObject("/Franka"); -shapehandles = sim.getObjectsInTree(handle, sim.object_shape_type, 0); - - -sim.stopSimulation(); \ No newline at end of file diff --git a/matlab/interfaces/coppeliasim/tests/test_class.m b/matlab/interfaces/coppeliasim/tests/test_class.m deleted file mode 100644 index b17fe69..0000000 --- a/matlab/interfaces/coppeliasim/tests/test_class.m +++ /dev/null @@ -1,17 +0,0 @@ -clear all -close all -clc - - -vi = DQ_CoppeliaSimInterface(); -vi.connect(); -vi.close_scene(); - -vi.load_from_model_browser("/robots/non-mobile/FrankaEmikaPanda.ttm", "/Franka"); - -vi.start_simulation() - - -vi.stop_simulation(); - - diff --git a/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterface.cpp b/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterface.cpp deleted file mode 100644 index fd50e9a..0000000 --- a/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterface.cpp +++ /dev/null @@ -1,2836 +0,0 @@ -/** -(C) Copyright 2024 DQ Robotics Developers - -This file is based on 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 . - -Contributors: -- Juan Jose Quiroz Omana - - Responsible for the original implementation. - The DQ_CoppeliaSimInterface class is partially based on the DQ_VrepInterface class - (https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_VrepInterface.h) - -*/ - -#include "dqrobotics/DQ.h" -#include -#include -#include - -//For C++20 -// #include -// #include - -//-------------------Private components-----------------------// -std::unique_ptr client_; -std::unique_ptr sim_; - -bool _create_client(const std::string& host = "localhost", - const int& rpcPort = 23000, - const int& cntPort = -1, - const int& verbose_ = -1, - const bool& client_flag = false); -void _set_status_bar_message(const std::string &message, const int& verbosity_type); - -//------------------------------------------------------------// - -/** - * @brief _set_status_bar_message - * @param message - * @param verbosity_type - */ -void _set_status_bar_message(const std::string &message, const int& verbosity_type) -{ - sim_->addLog(verbosity_type, message); -} - -/** - * @brief _create_client - * @param host - * @param rpcPort - * @param cntPort - * @param verbose_ - * @param client_flag - * @return - */ -bool _create_client(const std::string& host, - const int& rpcPort, - const int& cntPort, - const int& verbose_, - const bool& client_flag) -{ - if (!client_flag) - { - client_ = std::make_unique(host, rpcPort, cntPort, verbose_); - sim_ = std::make_unique(client_->getObject().sim()); - } - return true; -} -//-------------------------------------------------------------// - - -/** - * @brief DQ_CoppeliaSimInterface::DQ_CoppeliaSimInterface - */ -DQ_CoppeliaSimInterface::DQ_CoppeliaSimInterface() - :client_created_{false} -{ - -} - -/** - * @brief DQ_CoppeliaSimInterface::~DQ_CoppeliaSimInterface - */ -DQ_CoppeliaSimInterface::~DQ_CoppeliaSimInterface() -{ - _join_if_joinable_chronometer_thread(); - //if (is_simulation_running()) - // stop_simulation(); -} - - - - -/** - * @brief DQ_CoppeliaSimInterface::_join_if_joinable_chronometer_thread - */ -void DQ_CoppeliaSimInterface::_join_if_joinable_chronometer_thread() -{ - if (chronometer_thread_.joinable()) - chronometer_thread_.join(); -} - -/** - * @brief DQ_CoppeliaSimInterface::_start_chronometer - */ -void DQ_CoppeliaSimInterface::_start_chronometer() -{ - auto initial_time_ = std::chrono::steady_clock::now(); - while(elapsed_time_ < MAX_TIME_IN_MILLISECONDS_TO_TRY_CONNECTION_) - { - auto end{std::chrono::steady_clock::now()}; - auto elapsed_seconds_ = std::chrono::duration{end - initial_time_}; - elapsed_time_ = elapsed_seconds_.count()*1e3; - } - _check_connection(); -} - -/** - * @brief DQ_CoppeliaSimInterface::_check_connection - */ -void DQ_CoppeliaSimInterface::_check_connection() -{ - if (!client_created_) - { - //For C++20 - //std::cerr<verbosity_warnings); - } - catch (const std::runtime_error& e) - { - std::cerr << "Runtime error in DQ_CoppeliaSimInterface::connect. " - << e.what() << std::endl; - } - return client_created_; -} - -/** - * @brief DQ_CoppeliaSimInterface::_map_simulation_state - * @param state - * @return - */ -std::string DQ_CoppeliaSimInterface::_map_simulation_state(const int &state) -{ - return simulation_status_[state]; -} - -/** - * @brief DQ_CoppeliaSimInterface::start_simulation starts the CoppeliaSim simulation. - */ -void DQ_CoppeliaSimInterface::start_simulation() const -{ - _check_client(); - sim_->startSimulation(); -} - -/** - * @brief DQ_CoppeliaSimInterface::pause_simulation pauses the CoppeliaSim simulation. - */ -void DQ_CoppeliaSimInterface::pause_simulation() const -{ - _check_client(); - sim_->pauseSimulation(); - -} - - -/** - * @brief DQ_CoppeliaSimInterface::stop_simulation stops the CoppeliaSim simulation. - */ -void DQ_CoppeliaSimInterface::stop_simulation() const -{ - _check_client(); - sim_->stopSimulation(); -} - - -/** - * @brief DQ_CoppeliaSimInterface::set_stepping_mode enables or disables the stepping mode - * (formerly known as synchronous mode). - * @param flag. Eg: set_stepping_mode(true) // enables the stepping mode - * set_stepping_mode(false) // disables the stepping mode - */ -void DQ_CoppeliaSimInterface::set_stepping_mode(const bool &flag) -{ - _check_client(); - sim_->setStepping(flag); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_simulation_time returns the simulation time. - * This time does not correspond to the real-time necessarily. - * @return The simulation time. - */ -double DQ_CoppeliaSimInterface::get_simulation_time() const -{ - _check_client(); - return sim_->getSimulationTime(); -} - -/** - * @brief DQ_CoppeliaSimInterface::trigger_next_simulation_step This method sends a trigger - * signal to the CoppeliaSim scene, which performs a simulation step when the stepping mode is used. - */ -void DQ_CoppeliaSimInterface::trigger_next_simulation_step() const -{ - _check_client(); - sim_->step(); -} - - - -/** - * @brief DQ_CoppeliaSimInterface::is_simulation_running checks if the simulation is running. - * @return True if the simulation is running. False otherwise. - */ -bool DQ_CoppeliaSimInterface::is_simulation_running() const -{ - _check_client(); - return (sim_->getSimulationState() > sim_->simulation_paused); -} - - -/** - * @brief DQ_CoppeliaSimInterface::get_simulation_state - * See more in https://manual.coppeliarobotics.com/en/simulation.htm - * - * @return The simulation state. - * simulation_advancing = 16 - * simulation_advancing_abouttostop = 21 - * simulation_advancing_firstafterpause = 20 - * simulation_advancing_firstafterstop = 16 - * simulation_advancing_lastbeforepause = 19 - * simulation_advancing_lastbeforestop = 22 - * simulation_advancing_running = 17 - * simulation_paused = 8 - * simulation_stopped = 0 - * - */ -int DQ_CoppeliaSimInterface::get_simulation_state() const -{ - _check_client(); - return sim_->getSimulationState(); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_status_bar_message sends a message to CoppeliaSim to be - * displayed in the status bar. - * - * @param message - */ -void DQ_CoppeliaSimInterface::set_status_bar_message(const std::string &message) const -{ - _check_client(); - _set_status_bar_message(message, sim_->verbosity_undecorated); -} - - -/** - * @brief DQ_CoppeliaSimInterface::get_object_handle gets the object handle from - * CoppeliaSim. If the handle is not included in the map, then the map is - * updated. - * @param objectname - * @return the object handle. - */ -int DQ_CoppeliaSimInterface::get_object_handle(const std::string &objectname) -{ - int handle; - std::string additional_error_message = ""; - if (!_start_with_slash(objectname) && enable_deprecated_name_compatibility_ == false) - { - additional_error_message = std::string("Did you mean \"/" + objectname + "\"? \n"); - } - try - { - _check_client(); - auto standard_objectname = _get_standard_name(objectname); - handle = sim_->getObject(standard_objectname); - _update_map(standard_objectname, handle); - } - catch(const std::runtime_error& e) - { - auto error_msg = - std::string(e.what()) - + " \n" - + std::string("The object \"") - + objectname + std::string("\"") - + std::string(" does not exist in the current scene in CoppeliaSim. \n") - + additional_error_message; - _throw_runtime_error(error_msg); - } - return handle; -} - -/** - * @brief DQ_CoppeliaSimInterface::get_object_handles - * @param objectnames - * @return - */ -std::vector DQ_CoppeliaSimInterface::get_object_handles(const std::vector &objectnames) -{ - int n = objectnames.size(); - std::vector handles(n); - for(auto i=0;i DQ_CoppeliaSimInterface::get_map() -{ - return handles_map_; -} - -void DQ_CoppeliaSimInterface::show_map() -{ - for (const auto& p : handles_map_) - { - std::cout << '[' << p.first << "] = " << p.second << '\n'; - } -} - -void DQ_CoppeliaSimInterface::show_created_handle_map() -{ - std::cout<<"------------------"< "<getObjectPosition(handle, sim_->handle_world); - const DQ t = DQ(0, position.at(0),position.at(1),position.at(2)); - return t; -} - - -/** - * @brief DQ_CoppeliaSimInterface::get_object_translation returns a pure quaternion that represents the position - * of an object in the CoppeliaSim scene with respect to the absolute frame. - * @param objectname The name of the object. - * @return The position of the object. - */ -DQ DQ_CoppeliaSimInterface::get_object_translation(const std::string &objectname) -{ - return get_object_translation(_get_handle_from_map(objectname)); -} - - -/** - * @brief DQ_CoppeliaSimInterface::set_object_translation sets the translation of an object - * in the CoppeliaSim scene.. - * @param handle the object handle - * @param t The pure quaternion that represents the desired position with respect to the absolute frame.. - */ -void DQ_CoppeliaSimInterface::set_object_translation(const int &handle, const DQ &t) -{ - VectorXd vec_t = t.vec3(); - std::vector position = {vec_t[0], vec_t[1],vec_t[2]}; - _check_client(); - sim_->setObjectPosition(handle, position,sim_->handle_world); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_object_translation sets the translation of an object - * in the CoppeliaSim scene. - * @param objectname the name of the object - * @param t The pure quaternion that represents the desired position with respect to the absolute frame.. - */ -void DQ_CoppeliaSimInterface::set_object_translation(const std::string &objectname, const DQ &t) -{ - set_object_translation(_get_handle_from_map(objectname), t); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_object_rotation returns a unit quaternion that represents the rotation - * of an object in the CoppeliaSim scene with respect to the absolute frame. - * @param handle the object handle - * @return The object rotation. - */ -DQ DQ_CoppeliaSimInterface::get_object_rotation(const int &handle) const -{ - _check_client(); - auto rotation = sim_->getObjectQuaternion(handle + - sim_->handleflag_wxyzquat, - sim_->handle_world); - - return DQ(rotation.at(0), rotation.at(1), rotation.at(2), rotation.at(3)); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_object_rotation returns a unit quaternion that represents the rotation - * of an object in the CoppeliaSim scene with respect to the absolute frame. - * @param objectname the name of the object. - * @return The object rotation - */ -DQ DQ_CoppeliaSimInterface::get_object_rotation(const std::string &objectname) -{ - return get_object_rotation(_get_handle_from_map(objectname)); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_object_rotation sets the rotation of an object in the CoppeliaSim scene. - * @param handle the object handle - * @param r A unit quaternion that represents the desired rotation with respect to the absolute frame.. - */ -void DQ_CoppeliaSimInterface::set_object_rotation(const int &handle, const DQ &r) -{ - - VectorXd vec_r = r.vec4(); - std::vector rotation= {vec_r[0], vec_r[1],vec_r[2], vec_r[3]}; - _check_client(); - sim_->setObjectQuaternion(handle + sim_->handleflag_wxyzquat, rotation, sim_->handle_world); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_object_rotation sets the rotation of an object in the CoppeliaSim scene. - * @param objectname the name of the object - * @param r A unit quaternion that represents the desired rotation with respect to the absolute frame. - */ -void DQ_CoppeliaSimInterface::set_object_rotation(const std::string &objectname, const DQ &r) -{ - set_object_rotation(_get_handle_from_map(objectname), r); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_object_pose returns a unit dual quaternion that represents - * the object pose in the CoppeliaSim scene with respect to the absolute frame.. - * @param handle The object handle - * @return The desired object pose. - */ -DQ DQ_CoppeliaSimInterface::get_object_pose(const int &handle) const -{ - DQ t = get_object_translation(handle); - DQ r = get_object_rotation(handle); - return r + 0.5*E_*t*r; -} - -/** - * @brief DQ_CoppeliaSimInterface::get_object_pose returns a unit dual quaternion that represents - * the object pose in the CoppeliaSim scene with respect to the absolute frame. - * @param objectname The name of the object in the CoppeliaSim scene. - * @return the desired object pose - */ -DQ DQ_CoppeliaSimInterface::get_object_pose(const std::string &objectname) -{ - return get_object_pose(_get_handle_from_map(objectname)); -} - - -/** - * @brief DQ_CoppeliaSimInterface::set_object_pose sets the pose of an object in the CoppeliaSim scene. - * @param handle the object handle - * @param h A unit dual qualternion that represents the desired object pose with respect to the absolute frame. - */ -void DQ_CoppeliaSimInterface::set_object_pose(const int &handle, const DQ &h) const -{ - - VectorXd vec_r = h.P().vec4(); - VectorXd vec_p = h.translation().vec3(); - std::vector pose = {vec_p[0], vec_p[1],vec_p[2],vec_r[0], vec_r[1],vec_r[2], vec_r[3]}; - _check_client(); - sim_->setObjectPose(handle + sim_->handleflag_wxyzquat, pose, sim_->handle_world); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_object_pose sets the pose of an object in the CoppeliaSim scene. - * @param objectname The name of the object. - * @param h A unit dual qualternion that represents the desired object pose with respect to the absolute frame. - */ -void DQ_CoppeliaSimInterface::set_object_pose(const std::string &objectname, const DQ &h) -{ - // For C++20 - // std::string function_name = static_cast(std::source_location::current().function_name()); - std::string function_name = {"DQ_CoppeliaSimInterface::set_object_pose"}; - if (!is_unit(h)) - _throw_runtime_error(function_name + ". The pose must be a unit dual quaternion!"); - int handle = _get_handle_from_map(objectname); - set_object_pose(handle, h); -} - - -/** - * @brief DQ_CoppeliaSimInterface::get_joint_position - * @param handle - * @return - */ -double DQ_CoppeliaSimInterface::get_joint_position(const int &handle) const -{ - _check_client(); - return double(sim_->getJointPosition(handle)); -} - - -/** - * @brief DQ_CoppeliaSimInterface::get_joint_position - * @param jointname - * @return - */ -double DQ_CoppeliaSimInterface::get_joint_position(const std::string &jointname) -{ - return get_joint_position(_get_handle_from_map(jointname)); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_joint_positions - * @param handles - * @return - */ -VectorXd DQ_CoppeliaSimInterface::get_joint_positions(const std::vector &handles) const -{ - int n = handles.size(); - VectorXd joint_positions(n); - for(auto i=0;i &jointnames) -{ - int n = jointnames.size(); - VectorXd joint_positions(n); - for(auto i=0;isetJointPosition(handle, angle_rad); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_joint_position - * @param jointname - * @param angle_rad - */ -void DQ_CoppeliaSimInterface::set_joint_position(const std::string &jointname, const double &angle_rad) -{ - set_joint_position(_get_handle_from_map(jointname), angle_rad); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_joint_positions - * @param handles - * @param angles_rad - */ -void DQ_CoppeliaSimInterface::set_joint_positions(const std::vector &handles, const VectorXd &angles_rad) const -{ - for(std::size_t i=0;i &jointnames, const VectorXd &angles_rad) -{ - _check_sizes(jointnames, angles_rad, "Error in DQ_CoppeliaSimInterface::set_joint_positions: " - "jointnames and angles_rad have incompatible sizes"); - for(std::size_t i=0;isetJointTargetPosition(handle, angle_rad); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_joint_target_position - * @param jointname - * @param angle_rad - */ -void DQ_CoppeliaSimInterface::set_joint_target_position(const std::string &jointname, const double &angle_rad) -{ - set_joint_target_position(_get_handle_from_map(jointname), angle_rad); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_joint_target_positions - * @param handles - * @param angles_rad - */ -void DQ_CoppeliaSimInterface::set_joint_target_positions(const std::vector &handles, const VectorXd &angles_rad) const -{ - for(std::size_t i=0;i &jointnames, const VectorXd &angles_rad) -{ - _check_sizes(jointnames, angles_rad, "Error in DQ_CoppeliaSimInterface::set_joint_target_positions: " - "jointnames and angles_rad have incompatible sizes"); - for(std::size_t i=0;igetObjectFloatParam(handle, sim_->jointfloatparam_velocity); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_joint_velocity - * @param jointname - * @return - */ -double DQ_CoppeliaSimInterface::get_joint_velocity(const std::string &jointname) -{ - return get_joint_velocity(_get_handle_from_map(jointname)); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_joint_velocties - * @param handles - * @return - */ -VectorXd DQ_CoppeliaSimInterface::get_joint_velocities(const std::vector &handles) const -{ - int n = handles.size(); - VectorXd joint_velocities(n); - for(auto i=0;i &jointnames) -{ - int n = jointnames.size(); - VectorXd joint_velocities(n); - for(auto i=0;isetJointTargetVelocity(handle, angle_rad_dot); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_joint_target_velocity - * @param jointname - * @param angle_rad_dot - */ -void DQ_CoppeliaSimInterface::set_joint_target_velocity(const std::string &jointname, const double &angle_rad_dot) -{ - set_joint_target_velocity(_get_handle_from_map(jointname), angle_rad_dot); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_joint_target_velocities - * @param handles - * @param angles_rad_dot - */ -void DQ_CoppeliaSimInterface::set_joint_target_velocities(const std::vector &handles, const VectorXd &angles_rad_dot) const -{ - for(std::size_t i=0;i &jointnames, const VectorXd &angles_rad_dot) -{ - _check_sizes(jointnames, angles_rad_dot, "Error in DQ_CoppeliaSimInterface::set_joint_target_velocities: " - "jointnames and angles_rad_Dot have incompatible sizes"); - for(std::size_t i=0;isetJointTargetVelocity(handle, angle_dot_rad_max); - sim_->setJointTargetForce(handle, abs(torque)); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_joint_torque - * @param jointname - * @param torque - */ -void DQ_CoppeliaSimInterface::set_joint_torque(const std::string &jointname, const double &torque) -{ - set_joint_torque(_get_handle_from_map(jointname), torque); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_joint_torques - * @param handles - * @param torques - */ -void DQ_CoppeliaSimInterface::set_joint_torques(const std::vector &handles, const VectorXd &torques) const -{ - for(std::size_t i=0;i &jointnames, const VectorXd &torques) -{ - _check_sizes(jointnames, torques, "Error in DQ_CoppeliaSimInterface::set_joint_torques: " - "jointnames and torques have incompatible sizes"); - for(std::size_t i=0;igetJointForce(handle); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_joint_torque - * @param jointname - * @return - */ -double DQ_CoppeliaSimInterface::get_joint_torque(const std::string &jointname) -{ - return get_joint_torque(_get_handle_from_map(jointname)); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_joint_torques - * @param handles - * @return - */ -VectorXd DQ_CoppeliaSimInterface::get_joint_torques(const std::vector &handles) const -{ - int n = handles.size(); - VectorXd joint_torques(n); - for(auto i=0;i &jointnames) -{ - int n = jointnames.size(); - VectorXd joint_torques(n); - for(auto i=0;igetObjectAlias(handle, 1); - _update_map(objectname, handle); - return objectname; -} - - - - -/* - * --------------------------------------------------------------------------------------- -for C++20 -std::vector DQ_CoppeliaSimInterface::get_object_names(const auto &handles) -{ - int n = handles.size(); - std::vector objectnames(n); - for(auto i=0;i -std::vector DQ_CoppeliaSimInterface::get_object_names(const T &handles) -{ - int n = handles.size(); - std::vector objectnames(n); - for(auto i=0;i DQ_CoppeliaSimInterface::get_jointnames_from_parent_object(const std::string &parent_objectname) -{ - int base_handle = _get_handle_from_map(parent_objectname); - _check_client(); - std::vector jointhandles = sim_->getObjectsInTree(base_handle, - sim_->object_joint_type, - 0); - return get_object_names(jointhandles); - -} - -/** - * @brief DQ_CoppeliaSimInterface::get_shapenames_from_parent_object returns a vector containing all the shape names - * in which a specified object is its parent. - * @param parent_objectname The name of the object on CoppeliaSim that is the parent of the desired shapes. - * @param shape_type - * @return a vector containing the desired shape names - */ -std::vector DQ_CoppeliaSimInterface::get_shapenames_from_parent_object(const std::string &parent_objectname, - const SHAPE_TYPE &shape_type) -{ - int base_handle = _get_handle_from_map(parent_objectname); - _check_client(); - std::vector shapehandles = sim_->getObjectsInTree(base_handle, - sim_->object_shape_type, - 0); - size_t handlesizes = shapehandles.size(); - std::vector dynamic_features(handlesizes, 0); - - for (size_t i=0; igetObjectInt32Param(shapehandles.at(i), sim_->shapeintparam_static); - - std::vector aux_shapehandles; - switch (shape_type) { - case SHAPE_TYPE::DYNAMIC: - for (size_t i=0; i params = _get_velocity_const_params(); - VectorXd v = VectorXd::Zero(params.size()); - _check_client(); - for (size_t i=0; i < params.size(); i++) - { - v(i) = sim_->getObjectFloatParam(handle, params.at(i)); - } - if (reference == REFERENCE::BODY_FRAME) - { - DQ x = get_object_pose(handle); - DQ r = x.P(); - DQ w_b = r.conj()*DQ(v.head(3))*r; - DQ p_dot_b = r.conj()*DQ(v.tail(3))*r; - v.head(3) = w_b.vec3(); - v.tail(3) = p_dot_b.vec3(); - } - return v; -} - -/** - * @brief DQ_CoppeliaSimInterface::get_angular_and_linear_velocities - * @param objectname - * @param reference - * @return - */ -VectorXd DQ_CoppeliaSimInterface::get_angular_and_linear_velocities(std::string &objectname, const REFERENCE &reference) -{ - return get_angular_and_linear_velocities(_get_handle_from_map(objectname), reference); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_angular_and_linear_velocities - * @param handle - * @param w - * @param p_dot - * @param reference - */ -void DQ_CoppeliaSimInterface::set_angular_and_linear_velocities(const int &handle, const DQ &w, const DQ &p_dot, const REFERENCE &reference) const -{ - std::vector params = _get_velocity_const_params(); - VectorXd v = VectorXd::Zero(params.size()); - - if (reference == REFERENCE::ABSOLUTE_FRAME) - { - const DQ& w_a = w; - const DQ& p_dot_a = p_dot; - v.head(3) = w_a.vec3(); - v.tail(3) = p_dot_a.vec3(); - }else - { - DQ x = get_object_pose(handle); - DQ r = x.P(); - DQ w_a = r*w*r.conj(); - DQ p_dot_a = r*p_dot*r.conj(); - v.head(3) = w_a.vec3(); - v.tail(3) = p_dot_a.vec3(); - } - _check_client(); - sim_->resetDynamicObject(handle); - for (size_t i=0; i < params.size(); i++) - { - sim_->setObjectFloatParam(handle, params.at(i), v(i)); - } -} - -/** - * @brief DQ_CoppeliaSimInterface::set_angular_and_linear_velocities - * @param objectname - * @param w - * @param p_dot - * @param reference - */ -void DQ_CoppeliaSimInterface::set_angular_and_linear_velocities(std::string &objectname, const DQ &w, const DQ &p_dot, const REFERENCE &reference) -{ - set_angular_and_linear_velocities(_get_handle_from_map(objectname), w, p_dot, reference); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_twist - * @param handle - * @param reference - * @return - */ -DQ DQ_CoppeliaSimInterface::get_twist(const int &handle, const REFERENCE &reference) const -{ - VectorXd v = get_angular_and_linear_velocities(handle); - DQ w = DQ(v.head(3)); - DQ p_dot = DQ(v.tail(3)); - DQ x = get_object_pose(handle); - DQ twist = w + E_*(p_dot + cross(x.translation(), w));; - if (reference == REFERENCE::BODY_FRAME) - { - twist = x.conj()*twist*x; - } - return twist; -} - -/** - * @brief DQ_CoppeliaSimInterface::get_twist - * @param objectname - * @param reference - * @return - */ -DQ DQ_CoppeliaSimInterface::get_twist(const std::string &objectname, const REFERENCE &reference) -{ - return get_twist(_get_handle_from_map(objectname), reference); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_twist - * @param handle - * @param twist - * @param reference - */ -void DQ_CoppeliaSimInterface::set_twist(const int &handle, const DQ& twist, const REFERENCE &reference) const -{ - if (!is_pure(twist)) - { - throw(std::range_error("Bad set_object_twist() call: Not a pure dual quaternion")); - } - if (reference == REFERENCE::BODY_FRAME) - { - set_angular_and_linear_velocities(handle, twist.P(), twist.D(), - REFERENCE::BODY_FRAME); - } - else{ - DQ x = get_object_pose(handle); - set_angular_and_linear_velocities(handle, twist.P(), twist.D()-cross(x.translation(), twist.P()), - REFERENCE::ABSOLUTE_FRAME); - } -} - -/** - * @brief DQ_CoppeliaSimInterface::set_twist - * @param objectname - * @param twist - * @param reference - */ -void DQ_CoppeliaSimInterface::set_twist(const std::string &objectname, const DQ &twist, const REFERENCE &reference) -{ - set_twist(_get_handle_from_map(objectname), twist, reference); -} - - - - -/** - * @brief DQ_CoppeliaSimInterface::set_joint_mode - * @param jointname - * @param joint_mode - */ -void DQ_CoppeliaSimInterface::set_joint_mode(const std::string &jointname, const JOINT_MODE &joint_mode) -{ - _check_client(); - int jointMode; - switch (joint_mode) - { - case JOINT_MODE::KINEMATIC: - jointMode = sim_->jointmode_kinematic; - break; - case JOINT_MODE::DYNAMIC: - jointMode = sim_->jointmode_dynamic; - break; - case JOINT_MODE::DEPENDENT: - jointMode = sim_->jointmode_dependent; - break; - } - sim_->setJointMode(_get_handle_from_map(jointname), jointMode, 0); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_joint_modes - * @param jointnames - * @param joint_mode - */ -void DQ_CoppeliaSimInterface::set_joint_modes(const std::vector &jointnames, const JOINT_MODE &joint_mode) -{ - for(std::size_t i=0;ijointdynctrl_free; - break; - case JOINT_CONTROL_MODE::FORCE: - control_mode = sim_->jointdynctrl_force; - break; - case JOINT_CONTROL_MODE::VELOCITY: - control_mode = sim_->jointdynctrl_velocity; - break; - case JOINT_CONTROL_MODE::POSITION: - control_mode = sim_->jointdynctrl_position; - break; - case JOINT_CONTROL_MODE::SPRING: - control_mode = sim_->jointdynctrl_spring; - break; - case JOINT_CONTROL_MODE::CUSTOM: - control_mode = sim_->jointdynctrl_callback; - break; - case JOINT_CONTROL_MODE::TORQUE: - control_mode = sim_->jointdynctrl_velocity; - break; - } - sim_->setObjectInt32Param(_get_handle_from_map(jointname), - sim_->jointintparam_dynctrlmode, - control_mode); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_joint_control_modes - * @param jointnames - * @param joint_control_mode - */ -void DQ_CoppeliaSimInterface::set_joint_control_modes(const std::vector &jointnames, const JOINT_CONTROL_MODE &joint_control_mode) -{ - for(std::size_t i=0;isetBoolParam(sim_->boolparam_dynamics_handling_enabled, flag); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_simulation_time_step - * @return - */ -double DQ_CoppeliaSimInterface::get_simulation_time_step() const -{ - _check_client(); - return sim_->getFloatParam(sim_->floatparam_simulation_time_step); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_simulation_time_step - * @param time_step - */ -void DQ_CoppeliaSimInterface::set_simulation_time_step(const double &time_step) -{ - _check_client(); - sim_->setFloatParam(sim_->floatparam_simulation_time_step, time_step); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_physics_time_step - * @return - */ -double DQ_CoppeliaSimInterface::get_physics_time_step() const -{ - _check_client(); - return sim_->getFloatParam(sim_->floatparam_physicstimestep); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_physics_time_step - * @param time_step - */ -void DQ_CoppeliaSimInterface::set_physics_time_step(const double &time_step) const -{ - _check_client(); - sim_->setFloatParam(sim_->floatparam_physicstimestep, time_step); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_dynamic_engine - * @param engine - */ -void DQ_CoppeliaSimInterface::set_engine(const ENGINE &engine) -{ - _check_client(); - sim_->setInt32Param(sim_->intparam_dynamic_engine, engines_.at(engine)); -} - -std::string DQ_CoppeliaSimInterface::get_engine() -{ - switch (_get_engine()){ - case ENGINE::BULLET: - return "BULLET"; - case ENGINE::ODE: - return "ODE"; - case ENGINE::VORTEX: - return "VORTEX"; - case ENGINE::NEWTON: - return "NEWTON"; - case ENGINE::MUJOCO: - return "MUJOCO"; - default: // This line is required in GNU/Linux - _throw_runtime_error("wrong argument"); - } -} - -/** - * @brief DQ_CoppeliaSimInterface::set_gravity - * @param gravity - */ -void DQ_CoppeliaSimInterface::set_gravity(const DQ &gravity) -{ - VectorXd gravity_vec = gravity.vec3(); - std::vector g = {gravity_vec(0),gravity_vec(1), gravity_vec(2)}; - _check_client(); - sim_->setArrayParam(sim_->arrayparam_gravity, g); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_gravity - * @return - */ -DQ DQ_CoppeliaSimInterface::get_gravity() const -{ - _check_client(); - std::vector g = sim_->getArrayParam(sim_->arrayparam_gravity); - return DQ(0, g.at(0), g.at(1), g.at(2)); -} - -/** - * @brief DQ_CoppeliaSimInterface::load_scene loads a scene from your computer. - * @param path_to_filename the path to the scene. This string must containt - * the file extension. - * - * Example: - * - * load_scene("/Users/juanjqo/git/space_robot/scenes/space_robot.ttt"); - */ -void DQ_CoppeliaSimInterface::load_scene(const std::string &path_to_filename) const -{ - _check_client(); - sim_->loadScene(path_to_filename); -} - -/** - * @brief DQ_CoppeliaSimInterface::save_scene saves the current scene. - * @param path_to_filename The path where you want to save the scene including - * the name of the scene and its file extension. - * - * Example: - * - * save_scene("/Users/juanjqo/git/space_robot/scenes/space_robot2.ttt"); - */ -void DQ_CoppeliaSimInterface::save_scene(const std::string &path_to_filename) const -{ - _check_client(); - sim_->saveScene(path_to_filename); -} - -/** - * @brief DQ_CoppeliaSimInterface::close_scene closes the current scene. - */ -void DQ_CoppeliaSimInterface::close_scene() const -{ - _check_client(); - sim_->closeScene(); -} - -/** - * @brief DQ_CoppeliaSimInterface::load_model loads a model to - * the scene. - * - * @param path_to_filename The path to the model. - * @param desired_model_name The name you want for the loaded model. - * @param load_model_only_if_missing If the model exists (with the same alias) - * the model is not loaded. (Default) - * @param remove_child_script Remove the associated child script of the model - * (Default) - * @return A boolean flag. True if the model was loaded. False otherwise. - */ -bool DQ_CoppeliaSimInterface::load_model(const std::string &path_to_filename, - const std::string &desired_model_name, - const bool &load_model_only_if_missing, - const bool &remove_child_script) -{ - if (load_model_only_if_missing == true) - { - if (!object_exist_on_scene(std::string("/") + - _remove_first_slash_from_string(desired_model_name))) - { - return _load_model(path_to_filename, desired_model_name, remove_child_script); - }else - return true; - }else - {// Load the model even if the model is already on the scene - return _load_model(path_to_filename, desired_model_name, remove_child_script); - } - -} - -/** - * @brief DQ_CoppeliaSimInterface::load_from_model_browser loads a model from - * the CoppeliaSim model browser. - * - * Ex: load_from_model_browser("/robots/non-mobile/FrankaEmikaPanda.ttm", - "/Franka"); - * @param path_to_filename The path to the model relative to the model browser. - * @param desired_model_name The name you want for the loaded model. - * @param load_model_only_if_missing If the model exists (with the same alias) - * the model is not loaded. (Default) - * @param remove_child_script Remove the associated child script of the model - * (Default) - * @return A boolean flag. True if the model was loaded. False otherwise. - */ -bool DQ_CoppeliaSimInterface::load_from_model_browser(const std::string &path_to_filename, - const std::string &desired_model_name, - const bool &load_model_only_if_missing, - const bool &remove_child_script) -{ - _check_client(); - std::string resources_path = sim_->getStringParam(sim_->stringparam_resourcesdir); - return load_model(resources_path + std::string("/models") + path_to_filename, - desired_model_name, load_model_only_if_missing, remove_child_script); -} - -/** - * @brief DQ_CoppeliaSimInterface::remove_child_script_from_object - * The script must be located at objectname/script_name - * @param objectname - * @param script_name - */ -void DQ_CoppeliaSimInterface::remove_child_script_from_object(const std::string &objectname, const std::string &script_name) -{ - _check_client(); - if (object_exist_on_scene(_get_standard_name(objectname)+script_name)) - { - int handle = _get_handle_from_map(_get_standard_name(objectname)+script_name); - sim_->removeObjects({handle}, false); - } -} - -/** - * @brief DQ_CoppeliaSimInterface::object_exist_on_scene - * @param objectname - * @return - */ -bool DQ_CoppeliaSimInterface::object_exist_on_scene(const std::string &objectname) const -{ - std::optional options = {{"noError", false}}; - try { - _check_client(); - auto rtn = sim_->getObject(_get_standard_name(objectname), options); - return (rtn != -1) ? true : false; - } catch (...) { - return false; - } - -} - -void DQ_CoppeliaSimInterface::set_object_name(const int &handle, const std::string &new_object_name) const -{ - _check_client(); - sim_->setObjectAlias(handle, new_object_name); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_object_name - * @param current_object_name - * @param new_object_name - */ -void DQ_CoppeliaSimInterface::set_object_name(const std::string ¤t_object_name, const std::string &new_object_name) -{ - set_object_name(_get_handle_from_map(current_object_name), new_object_name); -} - - -void DQ_CoppeliaSimInterface::set_object_color(const int &handle, - const std::vector &rgba_color) const -{ - _check_client(); - sim_->setShapeColor(handle, "", sim_->colorcomponent_ambient_diffuse, {rgba_color.at(0), rgba_color.at(1),rgba_color.at(2)}); - sim_->setShapeColor(handle, "", sim_->colorcomponent_transparency, {rgba_color.at(3)}); -} - -void DQ_CoppeliaSimInterface::set_object_color(const std::string &objectname, const std::vector& rgba_color) -{ - // For C++20 - // std::string function_name = static_cast(std::source_location::current().function_name()); - std::string function_name = {"DQ_CoppeliaSimInterface::set_object_color"}; - if (rgba_color.size() != 4) - _throw_runtime_error(function_name + ". The rgba_color must be a vector of size 4."); - - set_object_color(_get_handle_from_map(objectname), rgba_color); -} - -void DQ_CoppeliaSimInterface::set_object_as_respondable(const int &handle, const bool &respondable_object) const -{ - _check_client(); - sim_->setObjectInt32Param(handle, - sim_->shapeintparam_respondable, - (respondable_object == true ? 1 : 0)); -} - - -void DQ_CoppeliaSimInterface::set_object_as_respondable(const std::string &objectname, const bool &respondable_object) -{ - set_object_as_respondable(_get_handle_from_map(objectname), respondable_object); -} - -void DQ_CoppeliaSimInterface::set_object_as_static(const int &handle, const bool &static_object) const -{ - _check_client(); - sim_->setObjectInt32Param(handle, - sim_->shapeintparam_static, - (static_object == true ? 1 : 0)); -} - -void DQ_CoppeliaSimInterface::set_object_as_static(const std::string &objectname, const bool &static_object) -{ - set_object_as_static(_get_handle_from_map(objectname), static_object); -} - - -int DQ_CoppeliaSimInterface::add_primitive(const PRIMITIVE &primitive, const std::string &name, - const std::vector &sizes) const -{ - if (!object_exist_on_scene(name)) - { - _check_client(); - int shapeHandle = sim_->createPrimitiveShape(get_primitive_identifier(primitive), sizes, 0); - set_object_name(shapeHandle, _remove_first_slash_from_string(name)); - return shapeHandle; - }else - return -1; - -} - - -/** - * @brief DQ_CoppeliaSimInterface::set_object_parent - * @param handle - * @param parent_handle - */ -void DQ_CoppeliaSimInterface::set_object_parent(const int &handle, const int &parent_handle, const bool &move_child_to_parent_pose) const -{ - _check_client(); - sim_->setObjectParent(handle, parent_handle, !move_child_to_parent_pose); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_object_parent - * @param objectname - * @param parent_object_name - */ -void DQ_CoppeliaSimInterface::set_object_parent(const std::string &objectname, - const std::string &parent_object_name, - const bool& move_child_to_parent_pose) -{ - set_object_parent(_get_handle_from_map(objectname), _get_handle_from_map(parent_object_name), move_child_to_parent_pose); -} - -/** - * @brief DQ_CoppeliaSimInterface::check_collision - * @param handle1 - * @param handle2 - * @return - */ -bool DQ_CoppeliaSimInterface::check_collision(const int &handle1, const int &handle2) const -{ - _check_client(); - auto [result, collidingObjectHandles] = sim_->checkCollision(handle1, handle2); - return result; -} - -/** - * @brief DQ_CoppeliaSimInterface::check_collision - * @param objectname1 - * @param objectname2 - * @return - */ -bool DQ_CoppeliaSimInterface::check_collision(const std::string &objectname1, const std::string &objectname2) -{ - return check_collision(_get_handle_from_map(objectname1), _get_handle_from_map(objectname2)); -} - -/** - * @brief DQ_CoppeliaSimInterface::check_distance - * @param handle1 - * @param handle2 - * @param threshold - * @return - */ -std::tuple DQ_CoppeliaSimInterface::check_distance(const int &handle1, const int &handle2, const double &threshold) const -{ - _check_client(); - auto [result, data, objectHandlePair] = sim_->checkDistance(handle1, handle2, threshold); - //[obj1X obj1Y obj1Z obj2X obj2Y obj2Z dist] - DQ point1 = DQ(0, data.at(0), data.at(1), data.at(2)); - DQ point2 = DQ(0, data.at(3), data.at(4), data.at(5)); - double distance = data.at(6); - return {distance, point1, point2}; -} - -/** - * @brief DQ_CoppeliaSimInterface::check_distance - * @param objectname1 - * @param objectname2 - * @param threshold - * @return - */ -std::tuple DQ_CoppeliaSimInterface::check_distance(const std::string &objectname1, const std::string &objectname2, const double &threshold) -{ - return check_distance(_get_handle_from_map(objectname1), _get_handle_from_map(objectname2), threshold); -} - -/** - * @brief DQ_CoppeliaSimInterface::compute_distance - * @param handle1 - * @param handle2 - * @param threshold - * @return - */ -double DQ_CoppeliaSimInterface::compute_distance(const int &handle1, const int &handle2, const double &threshold) const -{ - return std::get<0>(check_distance(handle1, handle2, threshold)); -} - -/** - * @brief DQ_CoppeliaSimInterface::compute_distance - * @param objectname1 - * @param objectname2 - * @param threshold - * @return - */ -double DQ_CoppeliaSimInterface::compute_distance(const std::string &objectname1, const std::string &objectname2, const double &threshold) -{ - return compute_distance(_get_handle_from_map(objectname1), _get_handle_from_map(objectname2), threshold); -} - - -/** - * @brief DQ_CoppeliaSimInterface::plot_plane - * @param name - * @param normal_to_the_plane - * @param location - * @param sizes - * @param rgba_color - * @param add_normal - * @param normal_scale - */ -void DQ_CoppeliaSimInterface::plot_plane(const std::string &name, - const DQ &normal_to_the_plane, - const DQ &location, - const std::vector &sizes, - const std::vector &rgba_color, - const bool &add_normal, - const double &normal_scale) -{ - // For C++20 - // std::string function_name = static_cast(std::source_location::current().function_name()); - std::string function_name = {"DQ_CoppeliaSimInterface::plot_plane"}; - - if (!is_unit(normal_to_the_plane) or !is_quaternion(normal_to_the_plane)) - _throw_runtime_error(function_name + ". The normal to the plane must be a unit quaternion!"); - if (!is_pure(location) or !is_quaternion(location)) - _throw_runtime_error(function_name + ". The location must be a pure quaternion!"); - - if (sizes.size() != 2) - _throw_runtime_error(function_name + ". The sizes must be vector of size 2."); - - if (rgba_color.size() != 4) - _throw_runtime_error(function_name + ". The rgba_color must be vector of size 4."); - - if (!object_exist_on_scene(name)) - { - _create_plane(name, sizes, rgba_color, add_normal, normal_scale); - } - set_object_pose(name, _get_pose_from_direction(normal_to_the_plane, location)); -} - - -void DQ_CoppeliaSimInterface::_create_plane(const std::string &name, const std::vector &sizes, const std::vector &rgba_color, const bool &add_normal, const double &normal_scale) const -{ - int primitive_handle = add_primitive(PRIMITIVE::PLANE, name, - {sizes.at(0), sizes.at(1), sizes.at(1)}); - set_object_color(primitive_handle, rgba_color); - set_object_as_respondable(primitive_handle, false); - set_object_as_static(primitive_handle, true); - std::vector children_names; - - if (add_normal) - { - double rfc = 0.02*normal_scale; - std::vector scaled_size = {rfc*sizes.at(0),rfc* sizes.at(1), 0.2*normal_scale*sizes.at(1)}; - children_names = _create_static_axis_at_origin(primitive_handle, name, scaled_size, AXIS::k, 1); - } - _merge_shapes(primitive_handle); - //_update_created_handles_map(name, children_names); -} - - - - -/** - * @brief DQ_CoppeliaSimInterface::plot_line - * @param name - * @param line_direction - * @param location - * @param thickness_and_length - * @param rgba_color - * @param add_arrow - * @param arrow_scale - */ -void DQ_CoppeliaSimInterface::plot_line(const std::string &name, const DQ &line_direction, const DQ &location, const std::vector &thickness_and_length, const std::vector &rgba_color, const bool &add_arrow, const double &arrow_scale) -{ - // For C++20 - // std::string function_name = static_cast(std::source_location::current().function_name()); - std::string function_name = {"DQ_CoppeliaSimInterface::plot_line"}; - - if (!is_unit(line_direction) or !is_quaternion(line_direction)) - _throw_runtime_error(function_name + ". The line direction must be a unit quaternion!"); - - if (!is_pure(location) or !is_quaternion(location)) - _throw_runtime_error(function_name + ". The location must be a pure quaternion!"); - - if (thickness_and_length.size() != 2) - _throw_runtime_error(function_name + ". The thickness_and_length must be vector of size 2."); - - if (rgba_color.size() != 4) - _throw_runtime_error(function_name + ". The rgba_color must be vector of size 4."); - - if (!object_exist_on_scene(name)) - { - _create_line(name, thickness_and_length, rgba_color, add_arrow, arrow_scale); - } - set_object_pose(name, _get_pose_from_direction(line_direction, location)); - -} - -void DQ_CoppeliaSimInterface::_create_line(const std::string &name, const std::vector &thickness_and_length, const std::vector &rgba_color, const bool &add_arrow, const double &arrow_scale) const -{ - int primitive_handle = add_primitive(PRIMITIVE::CYLINDER, name, - {thickness_and_length.at(0), thickness_and_length.at(0), thickness_and_length.at(1)}); - set_object_color(primitive_handle, rgba_color); - set_object_as_respondable(primitive_handle, false); - set_object_as_static(primitive_handle, true); - std::vector children_names; - if (add_arrow) - { - // Add the normal - - double rfc = 2*arrow_scale; - std::vector arrow_size = {rfc*thickness_and_length.at(0),rfc*thickness_and_length.at(0), 0.02*arrow_scale*thickness_and_length.at(1)}; - std::string arrow_name = _get_standard_name(name)+std::string("_normal"); - int arrow_handle = add_primitive(PRIMITIVE::CONE, arrow_name, arrow_size); - children_names.push_back(arrow_name); - _set_static_object_properties(arrow_handle, - primitive_handle , - 1+0.5*E_*0.5*thickness_and_length.at(1)*k_, - {0,0,1,1}); - } - _merge_shapes(primitive_handle); - //_update_created_handles_map(name, children_names); -} - - - - -/** - * @brief DQ_CoppeliaSimInterface::plot_cylinder - * @param name - * @param direction - * @param location - * @param width_and_length - * @param rgba_color - * @param add_line - * @param line_scale - */ -void DQ_CoppeliaSimInterface::plot_cylinder(const std::string &name, const DQ &direction, const DQ &location, const std::vector &width_and_length, const std::vector &rgba_color, const bool &add_line, const double &line_scale) -{ - // For C++20 - // std::string function_name = static_cast(std::source_location::current().function_name()); - std::string function_name = {"DQ_CoppeliaSimInterface::plot_cylinder"}; - if (!is_unit(direction) or !is_quaternion(direction)) - _throw_runtime_error(function_name + ". The line direction must be a unit quaternion!"); - - if (!is_pure(location) or !is_quaternion(location)) - _throw_runtime_error(function_name + ". The location must be a pure quaternion!"); - - if (width_and_length.size() != 2) - _throw_runtime_error(function_name + ". The thickness_and_length must be vector of size 2."); - - if (rgba_color.size() != 4) - _throw_runtime_error(function_name + ". The rgba_color must be vector of size 4."); - - if (!object_exist_on_scene(name)) - { - _create_cylinder(name, width_and_length, rgba_color, add_line, line_scale); - } - set_object_pose(name, _get_pose_from_direction(direction, location)); -} - - - -void DQ_CoppeliaSimInterface::_create_cylinder(const std::string &name, const std::vector &width_and_length, const std::vector &rgba_color, const bool &add_line, const double &line_scale) const -{ - int primitive_handle = add_primitive(PRIMITIVE::CYLINDER, name, - {width_and_length.at(0), width_and_length.at(0), width_and_length.at(1)}); - set_object_color(primitive_handle, rgba_color); - set_object_as_respondable(primitive_handle, false); - set_object_as_static(primitive_handle, true); - std::vector children_names; - std::string line_name = _get_standard_name(name)+std::string("_line"); - if (add_line) - { - double wscale = 0.05*line_scale; - double lscale = 1.1*line_scale; - int line_handle = add_primitive(PRIMITIVE::CYLINDER, line_name, - {wscale*width_and_length.at(0), wscale*width_and_length.at(0), lscale*width_and_length.at(1)}); - children_names.push_back(line_name); - _set_static_object_properties(line_handle, - primitive_handle, - DQ(1), - {0,0,1,1}); - - double rfc = 2*wscale*line_scale; - std::vector arrow_size = {rfc*width_and_length.at(0),rfc*width_and_length.at(0), 0.02*line_scale*width_and_length.at(1)}; - std::string arrow_name = _get_standard_name(name)+std::string("_normal"); - int arrow_handle = add_primitive(PRIMITIVE::CONE, arrow_name, arrow_size); - _set_static_object_properties(arrow_handle, - primitive_handle, - 1+0.5*E_*0.5*lscale*width_and_length.at(1)*k_, - {0,0,1,1}); - children_names.push_back(arrow_name); - - } - _merge_shapes(primitive_handle); - //_update_created_handles_map(name, children_names); -} - -/** - * @brief DQ_CoppeliaSimInterface::_merge_shapes - * @param parent_handle - */ -void DQ_CoppeliaSimInterface::_merge_shapes(const int &parent_handle) const -{ - std::vector shapehandles = sim_->getObjectsInTree(parent_handle, //_get_handle_from_map(name), - sim_->object_shape_type, - 0); - std::reverse(shapehandles.begin(), shapehandles.end()); - sim_->groupShapes(shapehandles, false); -} - -void DQ_CoppeliaSimInterface::plot_sphere(const std::string &name, const DQ &location, const double &size, const std::vector rgba_color) -{ - - // For C++20 - // std::string function_name = static_cast(std::source_location::current().function_name()); - std::string function_name = {"DQ_CoppeliaSimInterface::plot_sphere"}; - - if (!is_pure(location) or !is_quaternion(location)) - _throw_runtime_error(function_name + ". The location must be a pure quaternion!"); - if (rgba_color.size() != 4) - _throw_runtime_error(function_name + ". The rgba_color must be vector of size 4."); - - if (!object_exist_on_scene(name)) - { - int primitive_handle = add_primitive(PRIMITIVE::SPHEROID, name,{size, size, size}); - set_object_color(primitive_handle, rgba_color); - set_object_as_respondable(primitive_handle, false); - set_object_as_static(primitive_handle, true); - //std::vector children_names; - //_update_created_handles_map(name, children_names); - } - set_object_pose(name, 1+0.5*E_*location); -} - - -/** - * @brief DQ_CoppeliaSimInterface::plot_reference_frame - * @param name - * @param pose - * @param scale - * @param thickness_and_length - */ -void DQ_CoppeliaSimInterface::plot_reference_frame(const std::string &name, - const DQ &pose, - const double &scale, - const std::vector& thickness_and_length) -{ - _check_client(); - - // For C++20 - // std::string function_name = static_cast(std::source_location::current().function_name()); - std::string function_name = {"DQ_CoppeliaSimInterface::plot_reference_frame"}; - - if (!is_unit(pose)) - _throw_runtime_error(function_name + ". The pose must be a unit dual quaternion!"); - - if (thickness_and_length.size() != 2) - _throw_runtime_error(function_name + ". The thickness_and_length must be vector of size 2."); - - if (!object_exist_on_scene(name)) - { - _create_reference_frame(name, scale, thickness_and_length); - } - set_object_pose(name, pose); -} - - -void DQ_CoppeliaSimInterface::_create_reference_frame(const std::string &name, const double &scale, const std::vector &thickness_and_length) const -{ - int primitive_handle = add_primitive(PRIMITIVE::SPHEROID, name, - {1.5*scale*thickness_and_length.at(0), 1.5*scale*thickness_and_length.at(0), 1.5*scale*thickness_and_length.at(0)}); - set_object_color(primitive_handle, {1,1,1,0.5}); - set_object_as_respondable(primitive_handle, false); - set_object_as_static(primitive_handle, true); - std::vector children_names; - std::vector auxdest; - - std::vector scaled_size = {scale*thickness_and_length.at(0),scale*thickness_and_length.at(0), scale*thickness_and_length.at(1)}; - auto cnames1 = _create_static_axis_at_origin(primitive_handle, name, scaled_size, AXIS::k, 1); - auto cnames2 = _create_static_axis_at_origin(primitive_handle, name, scaled_size, AXIS::i, 1); - auto cnames3 = _create_static_axis_at_origin(primitive_handle, name, scaled_size, AXIS::j, 1); - - /* - std::set_union(cnames1.cbegin(), cnames1.cend(), - cnames2.cbegin(), cnames2.cend(), - std::back_inserter(auxdest)); - std::set_union(auxdest.cbegin(), auxdest.cend(), - cnames3.cbegin(), cnames3.cend(), - std::back_inserter(children_names)); - //_update_created_handles_map(name, children_names); - */ - - _merge_shapes(primitive_handle); -} - -/* - * @brief DQ_CoppeliaSimInterface::remove_plotted_object removes from the - * CoppeliaSim scene planes, lines , reference_frames or all objects - * added with plot-type methods. plot_plane, plot_line, and - * plot_reference_frame are examples of plot-type methods. - * @param name The name of the primitive to be removed - -void DQ_CoppeliaSimInterface::remove_plotted_object(const std::string &name) -{ - _check_client(); - auto standard_objectname = _get_standard_name(name); - //auto handle = _get_handle_from_map(standard_objectname); - - auto search = created_handles_map_.find(name); - if (search != created_handles_map_.end()) - { //found in map - auto children_names = search->second; - for (std::size_t i=0; i(std::source_location::current().function_name()); - std::string function_name = {"DQ_CoppeliaSimInterface::remove_plotted_object"}; - - _throw_runtime_error(function_name + ". The object "+name+" is not a plotted object or is not on the scene."); - } -} -*/ - -/** - * @brief DQ_CoppeliaSimInterface::draw_trajectory - * @param point - * @param size - * @param color - * @param max_item_count - */ -void DQ_CoppeliaSimInterface::draw_permanent_trajectory(const DQ &point, const double &size, const std::vector &color, const int &max_item_count) -{ - _check_client(); - if (!is_pure(point) or !is_quaternion(point)) - { - // For C++20 - // std::string function_name = static_cast(std::source_location::current().function_name()); - std::string function_name = {"DQ_CoppeliaSimInterface::draw_permanent_trajectory"}; - _throw_runtime_error(function_name + ". The point must be a pure quaternion."); - } - VectorXd vpoint = point.vec3(); - std::vector itemdata = {0,0,0,vpoint(0), vpoint(1), vpoint(2)}; - auto drawn_handle = sim_->addDrawingObject(sim_->drawing_lines+sim_->drawing_cyclic,size,0,-1,max_item_count, color); - sim_->addDrawingObjectItem( - drawn_handle, - itemdata - ); -} - -/** - * @brief DQ_CoppeliaSimInterface::add_simulation_lua_script - * @param script_name - * @param script_code - * @return - */ -int DQ_CoppeliaSimInterface::add_simulation_lua_script(const std::string &script_name, const std::string& script_code) -{ - _check_client(); - int scriptHandle = sim_->createScript(sim_->scripttype_simulation, - script_code, 0, "lua"); - set_object_name(scriptHandle, _remove_first_slash_from_string(script_name)); - return scriptHandle; -} - - -/** - * @brief DQ_CoppeliaSimInterface::draw_trajectory - * @param objectname - * @param size - * @param rgb_color - * @param max_item_count - */ -void DQ_CoppeliaSimInterface::draw_trajectory(const std::string &objectname, - const double &size, - const std::vector &rgb_color, - const int &max_item_count) -{ - - // For C++20 - // std::string function_name = static_cast(std::source_location::current().function_name()); - std::string function_name = {"DQ_CoppeliaSimInterface::draw_trajectory"}; - if (!object_exist_on_scene(objectname)) - _throw_runtime_error(function_name + ". The object " +objectname+ " is not on the scene."); - - if (!object_exist_on_scene(objectname+"/drawer")) - { - if (rgb_color.size() != 3) - _throw_runtime_error(function_name + ". The rgb_color must be vector of size 3."); - int r = rgb_color.at(0); - int g = rgb_color.at(1); - int b = rgb_color.at(2); - - std::string setting_str = " dr=sim.addDrawingObject(sim.drawing_lines|sim.drawing_cyclic,"+std::to_string(size)+",0,-1,"+std::to_string(max_item_count)+",{"+std::to_string(r)+","+std::to_string(g)+","+std::to_string(b)+"})" + "\n"; - - std::string code = - "function sysCall_init()" + std::string("\n") + - " h=sim.getObjectHandle(sim.handle_self)" + "\n" + - setting_str + - " pt=sim.getObjectPosition(h,-1) " + "\n" + - "end" + "\n" + - "function sysCall_sensing()" + "\n" + - " local l={pt[1],pt[2],pt[3]} " + "\n" + - " pt=sim.getObjectPosition(h,-1)" + "\n" + - " l[4]=pt[1]" + "\n" + - " l[5]=pt[2]" + "\n" + - " l[6]=pt[3]" + "\n" + - " sim.addDrawingObjectItem(dr,l)" + "\n" + - "end "; - add_simulation_lua_script("/drawer", code); - set_object_parent("/drawer", objectname); - } -} - - -/** - * @brief DQ_CoppeliaSimInterface::remove_object - * @param objectname - * @param remove_children - */ -void DQ_CoppeliaSimInterface::remove_object(const std::string& objectname, const bool &remove_children) -{ - _check_client(); - auto standard_objectname = _get_standard_name(objectname); - auto handle = _get_handle_from_map(standard_objectname); - - if (remove_children) - { - auto handles = sim_->getObjectsInTree(handle, sim_->handle_all, 0); - if (handles.size() > 0) - { - auto objectnames = get_object_names(handles); - sim_->removeObjects(handles, false); - for (std::size_t i=0; iremoveObjects({handle}, false); - _update_map(standard_objectname, handle, UPDATE_MAP::REMOVE); - } -} - -/** - * @brief DQ_CoppeliaSimInterface::get_bounding_box_size - * @param handle - * @return - */ -std::vector DQ_CoppeliaSimInterface::get_bounding_box_size(const int &handle) const -{ - _check_client(); - auto [size, pose] = sim_->getShapeBB(handle); - return size; -} - -/** - * @brief DQ_CoppeliaSimInterface::get_bounding_box_size - * @param objectname - * @return - */ -std::vector DQ_CoppeliaSimInterface::get_bounding_box_size(const std::string &objectname) -{ - return get_bounding_box_size(_get_handle_from_map(objectname)); -} - - -bool DQ_CoppeliaSimInterface::mujoco_is_used() -{ - return _get_engine() == ENGINE::MUJOCO ? true : false; -} - -/** - * @brief DQ_CoppeliaSimInterface::set_mujoco_global_impratio This attribute determines the ratio of - * frictional-to-normal constraint impedance for elliptic friction cones. - * The setting of solimp determines a single impedance value for - * all contact dimensions, which is then modulated by this attribute. - * Settings larger than 1 cause friction forces to be “harder” than - * normal forces, having the general effect of preventing slip, - * without increasing the actual friction coefficient. - * For pyramidal friction cones the situation is more complex because - * the pyramidal approximation mixes normal and frictional dimensions - * within each basis vector; it is not recommended to use high impratio - * values with pyramidal cones. - * - * @param impratio - */ -void DQ_CoppeliaSimInterface::set_mujoco_global_impratio(const double &impratio) -{ - sim_->setEngineFloatParam(sim_->mujoco_global_impratio,-1, impratio); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_mujoco_global_wind Velocity vector of the medium (i.e., wind). - * This vector is subtracted from the 3D translational velocity of each body, - * and the result is used to compute viscous, lift and drag forces acting on the body; - * @param wind - */ -void DQ_CoppeliaSimInterface::set_mujoco_global_wind(const std::vector &wind) -{ - std::vector mujoco_global_param= {sim_->mujoco_global_wind1, - sim_->mujoco_global_wind2, - sim_->mujoco_global_wind3}; - _check_sizes(wind, mujoco_global_param, "Error in DQ_CoppeliaSimInterface::set_mujoco_global_wind: " - "argument must be a vector of size "+std::to_string(mujoco_global_param.size())); - for (size_t i=0;isetEngineFloatParam(mujoco_global_param.at(i),-1, wind.at(i)); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_mujoco_global_density Density of the medium, not to be confused with the geom density used to infer masses and inertias. - * This parameter is used to simulate lift and drag forces, which scale quadratically with velocity. - * In SI units the density of air is around 1.2 while the density of water is around 1000 depending on temperature. Setting density to 0 disables lift and drag forces. - * @param density - */ -void DQ_CoppeliaSimInterface::set_mujoco_global_density(const double &density) -{ - sim_->setEngineFloatParam(sim_->mujoco_global_density,-1, density); -} - - -/** - * @brief DQ_CoppeliaSimInterface::set_mujoco_global_viscosity Viscosity of the medium. This parameter is used to simulate viscous forces, - * which scale linearly with velocity. - * In SI units the viscosity of air is around 0.00002 while the viscosity of water is around 0.0009 depending on temperature. - * Setting viscosity to 0 disables viscous forces. - * Note that the default Euler integrator handles damping in - * the joints implicitly – which improves stability and accuracy. - * It does not presently do this with body viscosity. - * Therefore, if the goal is merely to create a damped simulation - * (as opposed to modeling the specific effects of viscosity), - * we recommend using joint damping rather than body viscosity, - * or switching to the implicit or implicitfast integrators. - * @param viscosity - */ -void DQ_CoppeliaSimInterface::set_mujoco_global_viscosity(const double &viscosity) -{ - sim_->setEngineFloatParam(sim_->mujoco_global_viscosity,-1, viscosity); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_boundmass(const double &boundmass) -{ - sim_->setEngineFloatParam(sim_->mujoco_global_boundmass,-1, boundmass); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_boundinertia(const double &boundinertia) -{ - sim_->setEngineFloatParam(sim_->mujoco_global_boundinertia,-1, boundinertia); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_overridemargin(const double &overridemargin) -{ - sim_->setEngineFloatParam(sim_->mujoco_global_overridemargin,-1, overridemargin); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_overridesolref(const std::vector &overridesolref) -{ - std::vector mujoco_global_param = {sim_->mujoco_global_overridesolref1, - sim_->mujoco_global_overridesolref2}; - _check_sizes(overridesolref, mujoco_global_param, "Error in DQ_CoppeliaSimInterface::set_mujoco_global_overridesolref: " - "argument must be a vector of size "+std::to_string(mujoco_global_param.size())); - for (size_t i=0;isetEngineFloatParam(mujoco_global_param.at(i),-1, overridesolref.at(i)); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_overridesolimp(const std::vector &overridesolimp) -{ - std::vector mujoco_global_param = {sim_->mujoco_global_overridesolimp1, - sim_->mujoco_global_overridesolimp2, - sim_->mujoco_global_overridesolimp3, - sim_->mujoco_global_overridesolimp4, - sim_->mujoco_global_overridesolimp5, - }; - _check_sizes(overridesolimp, mujoco_global_param, "Error in DQ_CoppeliaSimInterface::set_mujoco_global_overridesolimp: " - "argument must be a vector of size "+std::to_string(mujoco_global_param.size())); - for (size_t i=0;isetEngineFloatParam(mujoco_global_param.at(i),-1, overridesolimp.at(i)); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_iterations(const int &iterations) -{ - sim_->setEngineInt32Param(sim_->mujoco_global_iterations,-1, iterations); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_integrator(const int &integrator) -{ - sim_->setEngineInt32Param(sim_->mujoco_global_integrator,-1, integrator); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_solver(const int &solver) -{ - sim_->setEngineInt32Param(sim_->mujoco_global_solver,-1, solver); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_njmax(const int &njmax) -{ - sim_->setEngineInt32Param(sim_->mujoco_global_njmax,-1, njmax); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_nstack(const int &nstack) -{ - sim_->setEngineInt32Param(sim_->mujoco_global_nstack,-1, nstack); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_nconmax(const int &nconmax) -{ - sim_->setEngineInt32Param(sim_->mujoco_global_nconmax,-1, nconmax); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_cone(const int &cone) -{ - sim_->setEngineInt32Param(sim_->mujoco_global_cone,-1, cone); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_overridekin(const int &overridekin) -{ - sim_->setEngineInt32Param(sim_->mujoco_global_overridekin,-1, overridekin); -} - -/* -void DQ_CoppeliaSimInterface::set_mujoco_global_rebuildcondition(const int &rebuildcondition) -{ - sim_->setEngineInt32Param(sim_->mujoco_global_rebuildcondition,-1, rebuildcondition); -} -*/ - -void DQ_CoppeliaSimInterface::set_mujoco_global_computeinertias(const bool &computeinertias) -{ - sim_->setEngineBoolParam(sim_->mujoco_global_computeinertias,-1, computeinertias); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_multithreaded(const bool &multithreaded) -{ - sim_->setEngineBoolParam(sim_->mujoco_global_multithreaded,-1, multithreaded); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_multiccd(const bool &multiccd) -{ - sim_->setEngineBoolParam(sim_->mujoco_global_multiccd,-1, multiccd); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_balanceinertias(const bool &balanceinertias) -{ - sim_->setEngineBoolParam(sim_->mujoco_global_balanceinertias,-1, balanceinertias); -} - -void DQ_CoppeliaSimInterface::set_mujoco_global_overridecontacts(const bool &overridecontacts) -{ - sim_->setEngineBoolParam(sim_->mujoco_global_overridecontacts,-1, overridecontacts); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_mujoco_joint_stiffness - * Joint stiffness. - * If this value is positive, a spring will be created with equilibrium position - * given by springref below. - * The spring force is computed along with the other passive forces. - * @param jointname - * @param stiffness - */ -void DQ_CoppeliaSimInterface::set_mujoco_joint_stiffness(const std::string &jointname, const double &stiffness) -{ - sim_->setEngineFloatParam(sim_->mujoco_joint_stiffness,_get_handle_from_map(jointname), stiffness); -} - -/** - * @brief DQ_CoppeliaSimInterface::set_mujoco_joint_stiffness - * Joint stiffness. - * If this value is positive, a spring will be created with equilibrium position - * given by springref below. - * The spring force is computed along with the other passive forces. - * @param jointnames - * @param stiffness - */ -void DQ_CoppeliaSimInterface::set_mujoco_joint_stiffnesses(const std::vector &jointnames, - const double &stiffness) -{ - for (size_t i=0;isetEngineFloatParam(sim_->mujoco_joint_damping, _get_handle_from_map(jointname), damping); -} - -void DQ_CoppeliaSimInterface::set_mujoco_joint_dampings(const std::vector &jointnames, - const double &damping) -{ - for (size_t i=0;isetEngineFloatParam(sim_->mujoco_joint_armature,_get_handle_from_map(jointname), armature); -} - -void DQ_CoppeliaSimInterface::set_mujoco_joint_armatures(const std::vector &jointnames, - const double &armature) -{ - for (size_t i=0;i &friction) -{ - std::vector mujoco_body_param = {sim_->mujoco_body_friction1, - sim_->mujoco_body_friction2, - sim_->mujoco_body_friction3}; - _check_sizes(friction, mujoco_body_param, "Error in DQ_CoppeliaSimInterface::set_mujoco_body_friction: " - "friction must be a vector of size "+std::to_string(mujoco_body_param.size())); - for (size_t i=0;isetEngineFloatParam(mujoco_body_param.at(i), _get_handle_from_map(bodyname),friction.at(i)); -} - -void DQ_CoppeliaSimInterface::set_mujoco_body_frictions(const std::vector &bodynames, const std::vector &friction) -{ - for (auto& bodyname : bodynames) - set_mujoco_body_friction(bodyname, friction); -} - - - - -/** - * @brief DQ_CoppeliaSimInterface::get_mass - * @param handle - * @return - */ -double DQ_CoppeliaSimInterface::get_mass(const int &handle) const -{ - _check_client(); - return sim_->getShapeMass(handle); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_mass - * @param object_name - * @return - */ -double DQ_CoppeliaSimInterface::get_mass(const std::string &object_name) -{ - return get_mass(_get_handle_from_map(object_name)); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_center_of_mass - * @param handle - * @param reference_frame - * @return - */ -DQ DQ_CoppeliaSimInterface::get_center_of_mass(const int &handle, const REFERENCE &reference_frame) const -{ - DQ COM_body_frame; - MatrixXd Inertia_maxtrix_body_frame; - std::tie(COM_body_frame, Inertia_maxtrix_body_frame) =_get_center_of_mass_and_inertia_matrix(handle); - if (reference_frame == REFERENCE::BODY_FRAME) - return COM_body_frame; - else - { - DQ x_0_bodyFrame = get_object_pose(handle); - DQ x_bodyFrame_com = 1 + 0.5*E_*COM_body_frame; - return (x_0_bodyFrame*x_bodyFrame_com).translation(); - } -} - -/** - * @brief DQ_CoppeliaSimInterface::get_center_of_mass - * @param object_name - * @param reference_frame - * @return - */ -DQ DQ_CoppeliaSimInterface::get_center_of_mass(const std::string &object_name, const REFERENCE &reference_frame) -{ - return get_center_of_mass(_get_handle_from_map(object_name), reference_frame); -} - -/** - * @brief DQ_CoppeliaSimInterface::get_inertia_matrix - * @param handle - * @param reference_frame - * @return - */ -MatrixXd DQ_CoppeliaSimInterface::get_inertia_matrix(const int &handle, const REFERENCE &reference_frame) -{ - DQ COM_body_frame; - MatrixXd Inertia_maxtrix_body_frame; - std::tie(COM_body_frame, Inertia_maxtrix_body_frame) =_get_center_of_mass_and_inertia_matrix(handle); - if (reference_frame == REFERENCE::BODY_FRAME) - return Inertia_maxtrix_body_frame; - else - { - DQ x_0_bodyFrame = get_object_pose(handle); - DQ x_bodyFrame_com = 1 + 0.5*E_*COM_body_frame; - DQ x_0_com = x_0_bodyFrame*x_bodyFrame_com; - MatrixXd R_0_COM = _get_rotation_matrix(x_0_com.P()); - return R_0_COM*Inertia_maxtrix_body_frame*R_0_COM.transpose(); - } -} - -/** - * @brief DQ_CoppeliaSimInterface::get_inertia_matrix - * @param link_name - * @param reference_frame - * @return - */ -MatrixXd DQ_CoppeliaSimInterface::get_inertia_matrix(const std::string &link_name, const REFERENCE &reference_frame) -{ - return get_inertia_matrix(_get_handle_from_map(link_name), reference_frame); -} - - -/** - * @brief DQ_CoppeliaSimInterface::_update_map updates the map if and only if - * the objectname is not in the map. - * @param objectname - * @param handle - */ -void DQ_CoppeliaSimInterface::_update_map(const std::string &objectname, - const int &handle, const UPDATE_MAP &mode) -{ - if (mode == DQ_CoppeliaSimInterface::UPDATE_MAP::ADD) - handles_map_.try_emplace(objectname, handle); - else - handles_map_.erase(objectname); - -} - - -/** - * @brief DQ_CoppeliaSimInterface::_get_handle_from_map searchs a handle in the map. - * - * @param objectname - * @return a tuple . If the handle is found in the map, returns . - * Otherwise, returns - */ -int DQ_CoppeliaSimInterface::_get_handle_from_map(const std::string &objectname) -{ - auto search = handles_map_.find(objectname); - if (search != handles_map_.end()) - { // handle found in map - return search->second; - } - else - { // handle not found in map. Therefore is taken from CoppeliaSim and the map - // is updated; - return get_object_handle(objectname); - } -} - -void DQ_CoppeliaSimInterface::_update_created_handles_map(const std::string &base_objectname, - const std::vector &children_objectnames, - const UPDATE_MAP &mode) -{ - if (mode == DQ_CoppeliaSimInterface::UPDATE_MAP::ADD) - created_handles_map_.try_emplace(base_objectname, children_objectnames); - else - created_handles_map_.erase(base_objectname); -} - -void DQ_CoppeliaSimInterface::_removed_handles_from_handles_map() -{ - -} - - - -//---------------Deprecated methods----------------------------- -void DQ_CoppeliaSimInterface::disconnect(){} -void DQ_CoppeliaSimInterface::disconnect_all(){} -void DQ_CoppeliaSimInterface::set_synchronous(const bool &flag){set_stepping_mode(flag);} -int DQ_CoppeliaSimInterface::wait_for_simulation_step_to_end(){return 0;} - - -//---------------Private methods----------------------------- -/** - * @brief DQ_CoppeliaSimInterface::_remove_first_slash_from_string - * @param str - * @return - */ -std::string DQ_CoppeliaSimInterface::_remove_first_slash_from_string(const std::string &str) const -{ - std::string new_str = str; - auto found = str.find('/'); - if (found != std::string::npos && found == 0) // The string containt the '/' in the first position - new_str.erase(0,1); // remove the '/' - return new_str; -} - -/** - * @brief DQ_CoppeliaSimInterface::_start_with_slash - * @param str - * @return - */ -bool DQ_CoppeliaSimInterface::_start_with_slash(const std::string &str) const -{ - - size_t found = str.find('/'); - if(found == 0) // The string containt the '/' - return true; - else - return false; - - //return str.starts_with("/"); - -} - -/** - * @brief DQ_CoppeliaSimInterface::_get_standard_string returns a string that - * always start with "/" - * @param str - * @return - */ -std::string DQ_CoppeliaSimInterface::_get_standard_name(const std::string &str) const -{ - std::string standard_str = str; - if (!_start_with_slash(str) && enable_deprecated_name_compatibility_ == true) - standard_str = std::string("/")+str; - return standard_str; -} - -DQ_CoppeliaSimInterface::ENGINE DQ_CoppeliaSimInterface::_get_engine() -{ - _check_client(); - int64_t eng = sim_->getInt32Param(sim_->intparam_dynamic_engine); - return engines_invmap.at(eng); -} - - -/** - * @brief DQ_CoppeliaSimInterface::_get_velocity_const_params - * @return - */ -std::vector DQ_CoppeliaSimInterface::_get_velocity_const_params() const -{ - std::vector params = {sim_->shapefloatparam_init_velocity_a, - sim_->shapefloatparam_init_velocity_b, - sim_->shapefloatparam_init_velocity_g, - sim_->shapefloatparam_init_velocity_x, - sim_->shapefloatparam_init_velocity_y, - sim_->shapefloatparam_init_velocity_z - }; - return params; -} - -/** - * @brief DQ_CoppeliaSimInterface::_load_model - * @param path_to_filename - * @param desired_model_name - * @return - */ -bool DQ_CoppeliaSimInterface::_load_model(const std::string &path_to_filename, - const std::string &desired_model_name, - const bool &remove_child_script) -{ - int rtn = sim_->loadModel(path_to_filename); - if (rtn != -1) - { - set_object_name(rtn, _remove_first_slash_from_string(desired_model_name)); - _get_handle_from_map(_get_standard_name(desired_model_name)); // This is to update the map only. - if (remove_child_script) - { - remove_child_script_from_object(std::string("/") - + _remove_first_slash_from_string(desired_model_name)); - } - return true; - }else{ - return false; - } -} - -/** - * @brief DQ_CoppeliaSimInterface::get_transformation_matrix - * @param coeff_vector - * @return - */ -MatrixXd DQ_CoppeliaSimInterface::_get_transformation_matrix(const std::vector &coeff_vector) const -{ - std::vector coeff = coeff_vector; - MatrixXd TM = Map(coeff.data(), - coeff.size()).reshaped(4,3).transpose(); - return TM; -} - -/** - * @brief This function returns a rotation matrix given a rotation unit quaternion. - * @param r unit quaternion. - * @returns The rotation matrix. - */ -MatrixXd DQ_CoppeliaSimInterface::_get_rotation_matrix(const DQ& r) const{ - Matrix R; - VectorXd vecr = r.vec4(); - double w = vecr(0); - double a = vecr(1); - double b = vecr(2); - double c = vecr(3); - R << 1-2*(b*b +c*c), 2*(a*b-w*c), 2*(a*c+w*b), - 2*(a*b+w*c), 1-2*(a*a+c*c), 2*(b*c-w*a), - 2*(a*c-w*b), 2*(b*c+w*a), 1-2*(a*a+b*b); - - return R; -} - -/** - * @brief DQ_CoppeliaSimInterface::_get_rotation_from_direction - * @param direction - * @return - */ -DQ DQ_CoppeliaSimInterface::_get_pose_from_direction(const DQ& direction, const DQ& point) -{ - DQ base_direction = k_; - DQ nx; - double argument = std::round(static_cast(dot(base_direction,direction.P()))*100000)/100000; - double phi = acos(argument); - if (phi == 0) - nx = base_direction; - else - nx = cross(base_direction, direction); - DQ r = cos(phi/2) + nx.normalize()*sin(phi/2); - return r + 0.5*E_*point*r; -} - -std::vector DQ_CoppeliaSimInterface::_create_static_axis_at_origin(const int& parent_handle, - const std::string& parent_name, - const std::vector& sizes, - const AXIS& axis, - const double& alpha_color) const -{ - std::vector created_primitives{}; - std::string name; - DQ dqaxis; - std::vector color; - DQ rotation; - const double angle = M_PI/2; // std::numbers::pi/2; - switch(axis) - { - case AXIS::i: - name = _get_standard_name(parent_name)+std::string("_x"); - dqaxis = i_; - color = {1,0,0,alpha_color}; - rotation = cos(angle/2) + j_*sin(angle/2); - break; - case AXIS::j: - name = _get_standard_name(parent_name)+std::string("_y"); - dqaxis = j_; - color = {0,1,0,alpha_color}; - rotation = cos(-angle/2) + i_*sin(-angle/2); - break; - case AXIS::k: - name = _get_standard_name(parent_name)+std::string("_z"); - dqaxis = k_; - color = {0,0,1,alpha_color}; - rotation = DQ(1); - break; - } - int primitive_handle = add_primitive(PRIMITIVE::CYLINDER,name,sizes); - created_primitives.push_back(name); - _set_static_object_properties(primitive_handle, - parent_handle, - rotation+0.5*E_*0.5*sizes.at(2)*dqaxis*rotation, - color - ); - std::vector arrow_size = {2*sizes.at(0), 2*sizes.at(1), 2*sizes.at(1)}; - std::string arrow_name = _get_standard_name(name)+std::string("_"); - int primitive_arrow = add_primitive(PRIMITIVE::CONE, arrow_name, arrow_size); - created_primitives.push_back(arrow_name); - _set_static_object_properties(primitive_arrow, - parent_handle , - rotation+0.5*E_*sizes.at(2)*dqaxis*rotation, - color); - return created_primitives; -} - -void DQ_CoppeliaSimInterface::_set_static_object_properties(const std::string &name, - const std::string &parent_name, - const DQ &pose, const std::vector &rgba_color) -{ - set_object_color(name, rgba_color); - set_object_as_respondable(name, false); - set_object_as_static(name, true); - set_object_pose(name, pose); - set_object_parent(name, parent_name, false); -} - -void DQ_CoppeliaSimInterface::_set_static_object_properties(const int &handle, const int &parent_handle, const DQ &pose, const std::vector &rgba_color) const -{ - set_object_color(handle, rgba_color); - set_object_as_respondable(handle, false); - set_object_as_static(handle, true); - set_object_pose(handle, pose); - set_object_parent(handle, parent_handle, false); -} - -int DQ_CoppeliaSimInterface::get_primitive_identifier(const PRIMITIVE &primitive) const -{ - switch (primitive) - { - case PRIMITIVE::PLANE: - return sim_->primitiveshape_plane; - case PRIMITIVE::DISC: - return sim_->primitiveshape_disc; - case PRIMITIVE::CUBOID: - return sim_->primitiveshape_cuboid; - case PRIMITIVE::SPHEROID: - return sim_->primitiveshape_spheroid; - case PRIMITIVE::CYLINDER: - return sim_->primitiveshape_cylinder; - case PRIMITIVE::CONE: - return sim_->primitiveshape_cone; - case PRIMITIVE::CAPSULE: - return sim_->primitiveshape_capsule; - default: // This line is required in GNU/Linux - _throw_runtime_error("wrong argument"); - - } -} - -void DQ_CoppeliaSimInterface::_check_client() const -{ - if (!client_created_) - throw std::runtime_error("Unestablished connection. Did you use connect()?"); -} - -[[noreturn]] void DQ_CoppeliaSimInterface::_throw_runtime_error(const std::string &msg) const -{ - stop_simulation(); - std::cerr<<"Something went wrong. I stopped the simulation!"< DQ_CoppeliaSimInterface::_get_center_of_mass_and_inertia_matrix(const int &handle) const -{ - std::vector inertia_matrix_coeff; - std::vector center_of_mass_coeff; - std::tie(inertia_matrix_coeff, center_of_mass_coeff) = sim_->getShapeInertia(handle); - - MatrixXd Inertia_maxtrix_body_frame = Map(inertia_matrix_coeff.data(), - inertia_matrix_coeff.size()).reshaped(3,3); - MatrixXd COM_body_frame_matrix = _get_transformation_matrix(center_of_mass_coeff); - DQ COM_body_frame = DQ(COM_body_frame_matrix.col(3)); - double mass = get_mass(handle); - - return {COM_body_frame, Inertia_maxtrix_body_frame/mass}; -} - - -//-------------------------------------------------------------- - diff --git a/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ.cpp b/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ.cpp new file mode 100644 index 0000000..8a2a6ea --- /dev/null +++ b/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ.cpp @@ -0,0 +1,961 @@ +/** +(C) Copyright 2024 DQ Robotics Developers + +This file is based on 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 . + +Contributors: +- Juan Jose Quiroz Omana + - Responsible for the original implementation. + The DQ_CoppeliaSimInterface class is partially based on the DQ_VrepInterface class + (https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_VrepInterface.h) + +*/ + +#include "dqrobotics/DQ.h" +#include +#include "internal/_zmq_wrapper.h" + +/** + * @brief _set_status_bar_message + * @param message + * @param verbosity_type + */ +void DQ_CoppeliaSimInterfaceZMQ::__set_status_bar_message(const std::string &message, const int& verbosity_type) const +{ + _ZMQWrapper::get_sim()->addLog(verbosity_type, message); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::DQ_CoppeliaSimInterfaceZMQ + */ +DQ_CoppeliaSimInterfaceZMQ::DQ_CoppeliaSimInterfaceZMQ() + :client_created_{false} +{ + +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::~DQ_CoppeliaSimInterfaceZMQ + */ +DQ_CoppeliaSimInterfaceZMQ::~DQ_CoppeliaSimInterfaceZMQ() +{ + _join_if_joinable_chronometer_thread(); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::_join_if_joinable_chronometer_thread + */ +void DQ_CoppeliaSimInterfaceZMQ::_join_if_joinable_chronometer_thread() +{ + if (chronometer_thread_.joinable()) + chronometer_thread_.join(); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::_start_chronometer + */ +void DQ_CoppeliaSimInterfaceZMQ::_start_chronometer() +{ + auto initial_time_ = std::chrono::steady_clock::now(); + while(elapsed_time_ < MAX_TIME_IN_MILLISECONDS_TO_TRY_CONNECTION_) + { + auto end{std::chrono::steady_clock::now()}; + auto elapsed_seconds_ = std::chrono::duration{end - initial_time_}; + elapsed_time_ = elapsed_seconds_.count()*1e3; + } + _check_connection(); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::_check_connection + */ +void DQ_CoppeliaSimInterfaceZMQ::_check_connection() +{ + if (!client_created_) + { + //For C++20 + //std::cerr<verbosity_warnings); + } + catch (const std::runtime_error& e) + { + std::cerr << "Runtime error in DQ_CoppeliaSimZmqInterface::connect. " + << e.what() << std::endl; + } + return client_created_; +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::connect establish a connection between the client (your code) and + * the host (the computer running the CoppeliaSim scene). + * Calling this method is required before anything else can happen. + * @param host The IP address of the computer that hosts the CoppeliaSim simulation. If the client (your code) + * and the simulation are running in the same computer, you can use "localhost". + * @param port The port to establish a connection. (e.g. 23000, 23001, 23002, 23003...). + * @param TIMEOUT_IN_MILISECONDS The timeout to establish the connection. + * @return true if the connection is established. False otherwise. + */ +bool DQ_CoppeliaSimInterfaceZMQ::connect(const std::string &host, const int &port, const int &TIMEOUT_IN_MILISECONDS) +{ + return _connect(host, port, TIMEOUT_IN_MILISECONDS, -1, -1); +} + +int DQ_CoppeliaSimInterfaceZMQ::_get_port_from_deprecated_default_port(const int &port) +{ + int auxport = port; + if (auxport == 19997 or auxport == 19998 or auxport == 19999 or auxport == 20000) + { + auxport = 23000; + std::cerr<<"The port "<startSimulation(); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::stop_simulation stops the CoppeliaSim simulation. + */ +void DQ_CoppeliaSimInterfaceZMQ::stop_simulation() const +{ + _check_client(); + _ZMQWrapper::get_sim()->stopSimulation(); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_stepping_mode enables or disables the stepping mode + * (formerly known as synchronous mode). + * @param flag. Eg: set_stepping_mode(true) // enables the stepping mode + * set_stepping_mode(false) // disables the stepping mode + */ +void DQ_CoppeliaSimInterfaceZMQ::set_stepping_mode(const bool &flag) const +{ + _check_client(); + _ZMQWrapper::get_sim()->setStepping(flag); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::trigger_next_simulation_step This method sends a trigger + * signal to the CoppeliaSim scene, which performs a simulation step when the stepping mode is used. + */ +void DQ_CoppeliaSimInterfaceZMQ::trigger_next_simulation_step() const +{ + _check_client(); + _ZMQWrapper::get_sim()->step(); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_status_bar_message sends a message to CoppeliaSim to be + * displayed in the status bar. + * + * @param message + */ +void DQ_CoppeliaSimInterfaceZMQ::_set_status_bar_message(const std::string &message) const +{ + _check_client(); + __set_status_bar_message(message, _ZMQWrapper::get_sim()->verbosity_undecorated); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_object_handle gets the object handle from + * CoppeliaSim. + * @param objectname The name of the object in the CoppeliaSim scene. + * @return the object handle. + */ +int DQ_CoppeliaSimInterfaceZMQ::get_object_handle(const std::string &objectname) +{ + int handle; + std::string additional_error_message = ""; + if (!_start_with_slash(objectname) && enable_deprecated_name_compatibility_ == false) + { + additional_error_message = std::string("Did you mean \"/" + objectname + "\"? \n"); + } + try + { + _check_client(); + auto standard_objectname = _get_standard_name(objectname); + handle = _ZMQWrapper::get_sim()->getObject(standard_objectname); + //If the handle is not included in the map, then the map is updated. + _update_map(standard_objectname, handle); + } + catch(const std::runtime_error& e) + { + auto error_msg = + std::string(e.what()) + + " \n" + + std::string("The object \"") + + objectname + std::string("\"") + + std::string(" does not exist in the current scene in CoppeliaSim. \n") + + additional_error_message; + _throw_runtime_error(error_msg); + } + return handle; +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_object_handles returns a vector containing the object handles. + * @param objectnames The vector that contains the object names in the CoppeliaSim scene. + * @return The desired vector of handles. + */ +std::vector DQ_CoppeliaSimInterfaceZMQ::get_object_handles(const std::vector &objectnames) +{ + int n = objectnames.size(); + std::vector handles(n); + for(auto i=0;igetObjectPosition(handle, _ZMQWrapper::get_sim()->handle_world); + const DQ t = DQ(0, position.at(0),position.at(1),position.at(2)); + return t; +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_object_translation returns a pure quaternion that represents the position + * of an object in the CoppeliaSim scene with respect to the absolute frame. + * @param objectname The name of the object. + * @return The position of the object. + */ +DQ DQ_CoppeliaSimInterfaceZMQ::get_object_translation(const std::string &objectname) +{ + return _get_object_translation(_get_handle_from_map(objectname)); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_object_translation sets the translation of an object + * in the CoppeliaSim scene.. + * @param handle the object handle + * @param t The pure quaternion that represents the desired position with respect to the absolute frame.. + */ +void DQ_CoppeliaSimInterfaceZMQ::_set_object_translation(const int &handle, const DQ &t) +{ + VectorXd vec_t = t.vec3(); + std::vector position = {vec_t[0], vec_t[1],vec_t[2]}; + _check_client(); + _ZMQWrapper::get_sim()->setObjectPosition(handle, position,_ZMQWrapper::get_sim()->handle_world); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_object_translation sets the translation of an object + * in the CoppeliaSim scene. + * @param objectname the name of the object + * @param t The pure quaternion that represents the desired position with respect to the absolute frame.. + */ +void DQ_CoppeliaSimInterfaceZMQ::set_object_translation(const std::string &objectname, const DQ &t) +{ + _set_object_translation(_get_handle_from_map(objectname), t); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_object_rotation returns a unit quaternion that represents the rotation + * of an object in the CoppeliaSim scene with respect to the absolute frame. Because the NEWTON engine + * generates unit quaternion norm issues, we used normalize(). + * @param handle the object handle + * @return The object rotation. + */ +DQ DQ_CoppeliaSimInterfaceZMQ::_get_object_rotation(const int &handle) const +{ + _check_client(); + auto rotation = _ZMQWrapper::get_sim()->getObjectQuaternion(handle + + _ZMQWrapper::get_sim()->handleflag_wxyzquat, + _ZMQWrapper::get_sim()->handle_world); + + return DQ(rotation.at(0), rotation.at(1), rotation.at(2), rotation.at(3)).normalize(); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_object_rotation returns a unit quaternion that represents the rotation + * of an object in the CoppeliaSim scene with respect to the absolute frame. + * @param objectname the name of the object. + * @return The object rotation + */ +DQ DQ_CoppeliaSimInterfaceZMQ::get_object_rotation(const std::string &objectname) +{ + return _get_object_rotation(_get_handle_from_map(objectname)); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_object_rotation sets the rotation of an object in the CoppeliaSim scene. + * @param handle the object handle + * @param r A unit quaternion that represents the desired rotation with respect to the absolute frame.. + */ +void DQ_CoppeliaSimInterfaceZMQ::_set_object_rotation(const int &handle, const DQ &r) +{ + + VectorXd vec_r = r.vec4(); + std::vector rotation= {vec_r[0], vec_r[1],vec_r[2], vec_r[3]}; + _check_client(); + _ZMQWrapper::get_sim()->setObjectQuaternion(handle + _ZMQWrapper::get_sim()->handleflag_wxyzquat, rotation, _ZMQWrapper::get_sim()->handle_world); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_object_rotation sets the rotation of an object in the CoppeliaSim scene. + * @param objectname the name of the object + * @param r A unit quaternion that represents the desired rotation with respect to the absolute frame. + */ +void DQ_CoppeliaSimInterfaceZMQ::set_object_rotation(const std::string &objectname, const DQ &r) +{ + _set_object_rotation(_get_handle_from_map(objectname), r); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_object_pose returns a unit dual quaternion that represents + * the object pose in the CoppeliaSim scene with respect to the absolute frame.. + * @param handle The object handle + * @return The desired object pose. + */ +DQ DQ_CoppeliaSimInterfaceZMQ::_get_object_pose(const int &handle) const +{ + DQ t = _get_object_translation(handle); + DQ r = _get_object_rotation(handle); + return r + 0.5*E_*t*r; +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_object_pose returns a unit dual quaternion that represents + * the object pose in the CoppeliaSim scene with respect to the absolute frame. + * @param objectname The name of the object in the CoppeliaSim scene. + * @return the desired object pose + */ +DQ DQ_CoppeliaSimInterfaceZMQ::get_object_pose(const std::string &objectname) +{ + return _get_object_pose(_get_handle_from_map(objectname)); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_object_pose sets the pose of an object in the CoppeliaSim scene. + * @param handle the object handle + * @param h A unit dual qualternion that represents the desired object pose with respect to the absolute frame. + */ +void DQ_CoppeliaSimInterfaceZMQ::_set_object_pose(const int &handle, const DQ &h) const +{ + + VectorXd vec_r = h.P().vec4(); + VectorXd vec_p = h.translation().vec3(); + std::vector pose = {vec_p[0], vec_p[1],vec_p[2],vec_r[0], vec_r[1],vec_r[2], vec_r[3]}; + _check_client(); + _ZMQWrapper::get_sim()->setObjectPose(handle + _ZMQWrapper::get_sim()->handleflag_wxyzquat, pose, _ZMQWrapper::get_sim()->handle_world); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_object_pose sets the pose of an object in the CoppeliaSim scene. + * @param objectname The name of the object. + * @param h A unit dual qualternion that represents the desired object pose with respect to the absolute frame. + */ +void DQ_CoppeliaSimInterfaceZMQ::set_object_pose(const std::string &objectname, const DQ &h) +{ + // For C++20 + // std::string function_name = static_cast(std::source_location::current().function_name()); + std::string function_name = {"DQ_CoppeliaSimInterface::set_object_pose"}; + if (!is_unit(h)) + _throw_runtime_error(function_name + ". The pose must be a unit dual quaternion!"); + int handle = _get_handle_from_map(objectname); + _set_object_pose(handle, h); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_joint_position gets the joint position in the CoppeliaSim scene + * @param handle The joint handle + * @return The joint position + */ +double DQ_CoppeliaSimInterfaceZMQ::_get_joint_position(const int &handle) const +{ + _check_client(); + return double(_ZMQWrapper::get_sim()->getJointPosition(handle)); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_joint_position gets the joint position in the CoppeliaSim scene + * @param jointname the joint name + * @return The joint position + */ +double DQ_CoppeliaSimInterfaceZMQ::_get_joint_position(const std::string &jointname) +{ + return _get_joint_position(_get_handle_from_map(jointname)); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_joint_positions gets the joint positions in the CoppeliaSim scene + * @param handles A vector containing the handles of the joints. + * @return The joint positions + */ +VectorXd DQ_CoppeliaSimInterfaceZMQ::_get_joint_positions(const std::vector &handles) const +{ + int n = handles.size(); + VectorXd joint_positions(n); + for(auto i=0;i &jointnames) +{ + int n = jointnames.size(); + VectorXd joint_positions(n); + for(auto i=0;isetJointPosition(handle, angle_rad); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_joint_position sets the joint position in the CoppeliaSim scene + * @param jointname The joint name + * @param angle_rad The desired joint position + */ +void DQ_CoppeliaSimInterfaceZMQ::_set_joint_position(const std::string &jointname, const double &angle_rad) +{ + _set_joint_position(_get_handle_from_map(jointname), angle_rad); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_joint_positions sets the joint positions in the CoppeliaSim scene + * @param handles A vector containing the joint handles + * @param angles_rad The desired joint positions + */ +void DQ_CoppeliaSimInterfaceZMQ::_set_joint_positions(const std::vector &handles, const VectorXd &angles_rad) const +{ + for(std::size_t i=0;i &jointnames, const VectorXd &angles_rad) +{ + _check_sizes(jointnames, angles_rad, "Error in DQ_CoppeliaSimInterface::set_joint_positions: " + "jointnames and angles_rad have incompatible sizes"); + for(std::size_t i=0;isetJointTargetPosition(handle, angle_rad); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_joint_target_position + * @param jointname + * @param angle_rad + */ +void DQ_CoppeliaSimInterfaceZMQ::_set_joint_target_position(const std::string &jointname, const double &angle_rad) +{ + _set_joint_target_position(_get_handle_from_map(jointname), angle_rad); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_joint_target_positions + * @param handles + * @param angles_rad + */ +void DQ_CoppeliaSimInterfaceZMQ::_set_joint_target_positions(const std::vector &handles, const VectorXd &angles_rad) const +{ + for(std::size_t i=0;i &jointnames, const VectorXd &angles_rad) +{ + _check_sizes(jointnames, angles_rad, "Error in DQ_CoppeliaSimInterface::set_joint_target_positions: " + "jointnames and angles_rad have incompatible sizes"); + for(std::size_t i=0;igetObjectFloatParam(handle, _ZMQWrapper::get_sim()->jointfloatparam_velocity); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_joint_velocity + * @param jointname + * @return + */ +double DQ_CoppeliaSimInterfaceZMQ::_get_joint_velocity(const std::string &jointname) +{ + return _get_joint_velocity(_get_handle_from_map(jointname)); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_joint_velocties + * @param handles + * @return + */ +VectorXd DQ_CoppeliaSimInterfaceZMQ::_get_joint_velocities(const std::vector &handles) const +{ + int n = handles.size(); + VectorXd joint_velocities(n); + for(auto i=0;i &jointnames) +{ + int n = jointnames.size(); + VectorXd joint_velocities(n); + for(auto i=0;isetJointTargetVelocity(handle, angle_rad_dot); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_joint_target_velocity + * @param jointname + * @param angle_rad_dot + */ +void DQ_CoppeliaSimInterfaceZMQ::_set_joint_target_velocity(const std::string &jointname, const double &angle_rad_dot) +{ + _set_joint_target_velocity(_get_handle_from_map(jointname), angle_rad_dot); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_joint_target_velocities + * @param handles + * @param angles_rad_dot + */ +void DQ_CoppeliaSimInterfaceZMQ::_set_joint_target_velocities(const std::vector &handles, const VectorXd &angles_rad_dot) const +{ + for(std::size_t i=0;i &jointnames, const VectorXd &angles_rad_dot) +{ + _check_sizes(jointnames, angles_rad_dot, "Error in DQ_CoppeliaSimInterface::set_joint_target_velocities: " + "jointnames and angles_rad_Dot have incompatible sizes"); + for(std::size_t i=0;isetJointTargetVelocity(handle, angle_dot_rad_max); + _ZMQWrapper::get_sim()->setJointTargetForce(handle, abs(torque)); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_joint_torque + * @param jointname + * @param torque + */ +void DQ_CoppeliaSimInterfaceZMQ::_set_joint_torque(const std::string &jointname, const double &torque) +{ + _set_joint_torque(_get_handle_from_map(jointname), torque); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::set_joint_torques + * @param handles + * @param torques + */ +void DQ_CoppeliaSimInterfaceZMQ::_set_joint_torques(const std::vector &handles, const VectorXd &torques) const +{ + for(std::size_t i=0;i &jointnames, const VectorXd &torques) +{ + _check_sizes(jointnames, torques, "Error in DQ_CoppeliaSimInterface::set_joint_torques: " + "jointnames and torques have incompatible sizes"); + for(std::size_t i=0;igetJointForce(handle); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_joint_torque + * @param jointname + * @return + */ +double DQ_CoppeliaSimInterfaceZMQ::_get_joint_torque(const std::string &jointname) +{ + return _get_joint_torque(_get_handle_from_map(jointname)); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::get_joint_torques + * @param handles + * @return + */ +VectorXd DQ_CoppeliaSimInterfaceZMQ::_get_joint_torques(const std::vector &handles) const +{ + int n = handles.size(); + VectorXd joint_torques(n); + for(auto i=0;i &jointnames) +{ + int n = jointnames.size(); + VectorXd joint_torques(n); + for(auto i=0;igetObjectAlias(handle, 1); + _update_map(objectname, handle); + return objectname; +} + +//----------------------------------------------------------------------------------------- + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::_update_map update the map. It the objectname is already in the map, and + * If you use the mode UPDATE_MAP::ADD, the map is updated only if the objectname is not in the map. + * In other words, it is not allowed to have a abjectname twice in the map. + * + * @param objectname The objectname you want to add or remove from the map. + * @param handle The objectname handle. + * @param mode The operation mode. Use UPDATE_MAP::ADD or UPDATE_MAP::REMOVE accordingly. + */ +void DQ_CoppeliaSimInterfaceZMQ::_update_map(const std::string &objectname, + const int &handle, const UPDATE_MAP &mode) +{ + if (mode == DQ_CoppeliaSimInterfaceZMQ::UPDATE_MAP::ADD) + handles_map_.try_emplace(objectname, handle); + else + handles_map_.erase(objectname); + +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::_get_handle_from_map searchs a handle in the map. If + * the handle is not found, it is taken from CoppeliaSim and the map is updated + * by using the get_object_handle() method. + * + * @param objectname + * @return The objectname handle. + */ +int DQ_CoppeliaSimInterfaceZMQ::_get_handle_from_map(const std::string &objectname) +{ + auto search = handles_map_.find(objectname); + // returns a tuple + //If the handle is found in the map, returns . + + + if (search != handles_map_.end()) + { // handle found in map + return search->second; + } + else + { // handle not found in map. Therefore, it is taken from CoppeliaSim and the map + // is updated; + return get_object_handle(objectname); + } +} + + +//---------------Deprecated methods----------------------------- +void DQ_CoppeliaSimInterfaceZMQ::disconnect(){} +void DQ_CoppeliaSimInterfaceZMQ::disconnect_all(){} +void DQ_CoppeliaSimInterfaceZMQ::set_synchronous(const bool &flag){set_stepping_mode(flag);} +int DQ_CoppeliaSimInterfaceZMQ::wait_for_simulation_step_to_end(){return 0;} + +bool DQ_CoppeliaSimInterfaceZMQ::connect(const int &port, + const int &TIMEOUT_IN_MILISECONDS, + const int &MAX_TRY_COUNT) +{ + return connect("localhost", _get_port_from_deprecated_default_port(port), TIMEOUT_IN_MILISECONDS); +} + +bool DQ_CoppeliaSimInterfaceZMQ::connect(const std::string &ip, + const int &port, + const int &TIMEOUT_IN_MILISECONDS, + const int &MAX_TRY_COUNT) +{ + return connect(ip, _get_port_from_deprecated_default_port(port), TIMEOUT_IN_MILISECONDS); +} + + +//---------------Private methods----------------------------- +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::_remove_first_slash_from_string this method removes the slash at the beginning of a string. + * + * Example: _remove_first_slash_from_string("/reference") returns "reference" + * _remove_first_slash_from_string("reference") returns "reference" + * + * @param str + * @return + */ +std::string DQ_CoppeliaSimInterfaceZMQ::_remove_first_slash_from_string(const std::string &str) const +{ + std::string new_str = str; + auto found = str.find('/'); + if (found != std::string::npos && found == 0) // The string containt the '/' in the first position + new_str.erase(0,1); // remove the '/' + return new_str; +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::_start_with_slash returns true if the first character of a string is a slash. + * Otherwise returns false + * @param str The string + * @return A boolean. True if the first character of a string is a slash. False otherwise + */ +bool DQ_CoppeliaSimInterfaceZMQ::_start_with_slash(const std::string &str) const +{ + + size_t found = str.find('/'); + if(found == 0) // The string containt the '/' + return true; + else + return false; + + //return str.starts_with("/"); // For C++20 + +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::_get_standard_string returns a string that + * always start with "/" + * @param str + * @return + */ +std::string DQ_CoppeliaSimInterfaceZMQ::_get_standard_name(const std::string &str) const +{ + std::string standard_str = str; + if (!_start_with_slash(str) && enable_deprecated_name_compatibility_ == true) + standard_str = std::string("/")+str; + return standard_str; +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQ::_check_client checks if the client is initialized. + */ +void DQ_CoppeliaSimInterfaceZMQ::_check_client() const +{ + if (!client_created_) + throw std::runtime_error("Unestablished connection. Did you use connect()?"); +} + +[[noreturn]] void DQ_CoppeliaSimInterfaceZMQ::_throw_runtime_error(const std::string &msg) const +{ + stop_simulation(); + std::cerr<<"Something went wrong. I stopped the simulation!"<. + +Contributors: +- Juan Jose Quiroz Omana + - Responsible for the original implementation. + The DQ_CoppeliaSimInterface class is partially based on the DQ_VrepInterface class + (https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_VrepInterface.h) + +*/ + +#include "dqrobotics/DQ.h" +#include +#include +#include "internal/_zmq_wrapper.h" + + +/** + * @brief set_status_bar_message + * @param message + * @param verbosity_type + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::__set_status_bar_message(const std::string &message, const int& verbosity_type) const +{ + _ZMQWrapper::get_sim()->addLog(verbosity_type, message); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::DQ_CoppeliaSimInterfaceZMQExperimental + */ +DQ_CoppeliaSimInterfaceZMQExperimental::DQ_CoppeliaSimInterfaceZMQExperimental() + :DQ_CoppeliaSimInterfaceZMQ() +{ + +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::~DQ_CoppeliaSimInterfaceZMQExperimental + */ +DQ_CoppeliaSimInterfaceZMQExperimental::~DQ_CoppeliaSimInterfaceZMQExperimental() +{ + _join_if_joinable_chronometer_thread(); +} + + + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::pause_simulation pauses the CoppeliaSim simulation. + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::pause_simulation() const +{ + _check_client(); + _ZMQWrapper::get_sim()->pauseSimulation(); + +} + + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_simulation_time returns the simulation time. + * This time does not correspond to the real-time necessarily. + * @return The simulation time. + */ +double DQ_CoppeliaSimInterfaceZMQExperimental::get_simulation_time() const +{ + _check_client(); + return _ZMQWrapper::get_sim()->getSimulationTime(); +} + + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::is_simulation_running checks if the simulation is running. + * @return True if the simulation is running. False otherwise. + */ +bool DQ_CoppeliaSimInterfaceZMQExperimental::is_simulation_running() const +{ + _check_client(); + return (_ZMQWrapper::get_sim()->getSimulationState() > _ZMQWrapper::get_sim()->simulation_paused); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_simulation_state returns the simulation state + * See more in https://manual.coppeliarobotics.com/en/simulation.htm + * + * @return The simulation state. + * simulation_advancing = 16 + * simulation_advancing_abouttostop = 21 + * simulation_advancing_firstafterpause = 20 + * simulation_advancing_firstafterstop = 16 + * simulation_advancing_lastbeforepause = 19 + * simulation_advancing_lastbeforestop = 22 + * simulation_advancing_running = 17 + * simulation_paused = 8 + * simulation_stopped = 0 + * + */ +int DQ_CoppeliaSimInterfaceZMQExperimental::get_simulation_state() const +{ + _check_client(); + return _ZMQWrapper::get_sim()->getSimulationState(); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_status_bar_message sends a message to CoppeliaSim to be + * displayed in the status bar. + * + * @param message + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_status_bar_message(const std::string &message) const +{ + _check_client(); + __set_status_bar_message(message, _ZMQWrapper::get_sim()->verbosity_undecorated); +} + + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_jointnames_from_parent_object returns a vector containing all the joint names + * in which a specified object is its parent. + * @param parent_objectname The name of the object on CoppeliaSim that is the parent of the desired joints. + * @return a vector containing the desired joint names + */ +std::vector DQ_CoppeliaSimInterfaceZMQExperimental::get_jointnames_from_parent_object(const std::string &parent_objectname) +{ + int base_handle = _get_handle_from_map(parent_objectname); + _check_client(); + std::vector jointhandles = _ZMQWrapper::get_sim()->getObjectsInTree(base_handle, + _ZMQWrapper::get_sim()->object_joint_type, + 0); + return _get_object_names(jointhandles); + +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_shapenames_from_parent_object returns a vector containing all the shape names + * in which a specified object is its parent. + * @param parent_objectname The name of the object on CoppeliaSim that is the parent of the desired shapes. + * @param shape_type + * @return a vector containing the desired shape names + */ +std::vector DQ_CoppeliaSimInterfaceZMQExperimental::get_shapenames_from_parent_object(const std::string &parent_objectname, + const SHAPE_TYPE &shape_type) +{ + int base_handle = _get_handle_from_map(parent_objectname); + _check_client(); + std::vector shapehandles = _ZMQWrapper::get_sim()->getObjectsInTree(base_handle, + _ZMQWrapper::get_sim()->object_shape_type, + 0); + size_t handlesizes = shapehandles.size(); + std::vector dynamic_features(handlesizes, 0); + + for (size_t i=0; igetObjectInt32Param(shapehandles.at(i), _ZMQWrapper::get_sim()->shapeintparam_static); + + std::vector aux_shapehandles; + switch (shape_type) { + case SHAPE_TYPE::DYNAMIC: + for (size_t i=0; i params = get_velocity_const_params(); + VectorXd v = VectorXd::Zero(params.size()); + _check_client(); + for (size_t i=0; i < params.size(); i++) + { + v(i) = _ZMQWrapper::get_sim()->getObjectFloatParam(handle, params.at(i)); + } + if (reference == REFERENCE::BODY_FRAME) + { + DQ x = _get_object_pose(handle); + DQ r = x.P(); + DQ w_b = r.conj()*DQ(v.head(3))*r; + DQ p_dot_b = r.conj()*DQ(v.tail(3))*r; + v.head(3) = w_b.vec3(); + v.tail(3) = p_dot_b.vec3(); + } + return v; +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_get_angular_and_linear_velocities returns the angular a linear velocities of an + * object in the CoppeliaSim scene. + * @param objectname The object name + * @param reference The reference frame. + * @return A vector containg the angular and linear velocities. [wx wy wz x_dot y_dot z_dot] + */ +VectorXd DQ_CoppeliaSimInterfaceZMQExperimental::get_angular_and_linear_velocities(std::string &objectname, const REFERENCE &reference) +{ + return get_angular_and_linear_velocities(_get_handle_from_map(objectname), reference); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_set_angular_and_linear_velocities + * @param handle + * @param w + * @param p_dot + * @param reference + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_angular_and_linear_velocities(const int &handle, const DQ &w, const DQ &p_dot, const REFERENCE &reference) const +{ + std::vector params = get_velocity_const_params(); + VectorXd v = VectorXd::Zero(params.size()); + + if (reference == REFERENCE::ABSOLUTE_FRAME) + { + const DQ& w_a = w; + const DQ& p_dot_a = p_dot; + v.head(3) = w_a.vec3(); + v.tail(3) = p_dot_a.vec3(); + }else + { + DQ x = _get_object_pose(handle); + DQ r = x.P(); + DQ w_a = r*w*r.conj(); + DQ p_dot_a = r*p_dot*r.conj(); + v.head(3) = w_a.vec3(); + v.tail(3) = p_dot_a.vec3(); + } + _check_client(); + _ZMQWrapper::get_sim()->resetDynamicObject(handle); + for (size_t i=0; i < params.size(); i++) + { + _ZMQWrapper::get_sim()->setObjectFloatParam(handle, params.at(i), v(i)); + } +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_set_angular_and_linear_velocities + * @param objectname + * @param w + * @param p_dot + * @param reference + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_angular_and_linear_velocities(std::string &objectname, const DQ &w, const DQ &p_dot, const REFERENCE &reference) +{ + set_angular_and_linear_velocities(_get_handle_from_map(objectname), w, p_dot, reference); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_get_twist + * @param handle + * @param reference + * @return + */ +DQ DQ_CoppeliaSimInterfaceZMQExperimental::get_twist(const int &handle, const REFERENCE &reference) const +{ + VectorXd v = get_angular_and_linear_velocities(handle); + DQ w = DQ(v.head(3)); + DQ p_dot = DQ(v.tail(3)); + DQ x = _get_object_pose(handle); + DQ twist = w + E_*(p_dot + cross(x.translation(), w));; + if (reference == REFERENCE::BODY_FRAME) + { + twist = x.conj()*twist*x; + } + return twist; +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_get_twist + * @param objectname + * @param reference + * @return + */ +DQ DQ_CoppeliaSimInterfaceZMQExperimental::get_twist(const std::string &objectname, const REFERENCE &reference) +{ + return get_twist(_get_handle_from_map(objectname), reference); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_set_twist + * @param handle + * @param twist + * @param reference + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_twist(const int &handle, const DQ& twist, const REFERENCE &reference) const +{ + if (!is_pure(twist)) + { + throw(std::range_error("Bad set_object_twist() call: Not a pure dual quaternion")); + } + if (reference == REFERENCE::BODY_FRAME) + { + set_angular_and_linear_velocities(handle, twist.P(), twist.D(), + REFERENCE::BODY_FRAME); + } + else{ + DQ x = _get_object_pose(handle); + set_angular_and_linear_velocities(handle, twist.P(), twist.D()-cross(x.translation(), twist.P()), + REFERENCE::ABSOLUTE_FRAME); + } +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_set_twist + * @param objectname + * @param twist + * @param reference + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_twist(const std::string &objectname, const DQ &twist, const REFERENCE &reference) +{ + set_twist(_get_handle_from_map(objectname), twist, reference); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_joint_mode + * @param jointname + * @param joint_mode + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_joint_mode(const std::string &jointname, const JOINT_MODE &joint_mode) +{ + _check_client(); + int jointMode; + switch (joint_mode) + { + case JOINT_MODE::KINEMATIC: + jointMode = _ZMQWrapper::get_sim()->jointmode_kinematic; + break; + case JOINT_MODE::DYNAMIC: + jointMode = _ZMQWrapper::get_sim()->jointmode_dynamic; + break; + case JOINT_MODE::DEPENDENT: + jointMode = _ZMQWrapper::get_sim()->jointmode_dependent; + break; + } + _ZMQWrapper::get_sim()->setJointMode(_get_handle_from_map(jointname), jointMode, 0); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_joint_modes + * @param jointnames + * @param joint_mode + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_joint_modes(const std::vector &jointnames, const JOINT_MODE &joint_mode) +{ + for(std::size_t i=0;ijointdynctrl_free; + break; + case JOINT_CONTROL_MODE::FORCE: + control_mode = _ZMQWrapper::get_sim()->jointdynctrl_force; + break; + case JOINT_CONTROL_MODE::VELOCITY: + control_mode = _ZMQWrapper::get_sim()->jointdynctrl_velocity; + break; + case JOINT_CONTROL_MODE::POSITION: + control_mode = _ZMQWrapper::get_sim()->jointdynctrl_position; + break; + case JOINT_CONTROL_MODE::SPRING: + control_mode = _ZMQWrapper::get_sim()->jointdynctrl_spring; + break; + case JOINT_CONTROL_MODE::CUSTOM: + control_mode = _ZMQWrapper::get_sim()->jointdynctrl_callback; + break; + case JOINT_CONTROL_MODE::TORQUE: + control_mode = _ZMQWrapper::get_sim()->jointdynctrl_velocity; + break; + } + _ZMQWrapper::get_sim()->setObjectInt32Param(_get_handle_from_map(jointname), + _ZMQWrapper::get_sim()->jointintparam_dynctrlmode, + control_mode); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_joint_control_modes + * @param jointnames + * @param joint_control_mode + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_joint_control_modes(const std::vector &jointnames, const JOINT_CONTROL_MODE &joint_control_mode) +{ + for(std::size_t i=0;isetBoolParam(_ZMQWrapper::get_sim()->boolparam_dynamics_handling_enabled, flag); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_simulation_time_step + * @return + */ +double DQ_CoppeliaSimInterfaceZMQExperimental::get_simulation_time_step() const +{ + _check_client(); + return _ZMQWrapper::get_sim()->getFloatParam(_ZMQWrapper::get_sim()->floatparam_simulation_time_step); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_simulation_time_step + * @param time_step + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_simulation_time_step(const double &time_step) +{ + _check_client(); + _ZMQWrapper::get_sim()->setFloatParam(_ZMQWrapper::get_sim()->floatparam_simulation_time_step, time_step); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_physics_time_step + * @return + */ +double DQ_CoppeliaSimInterfaceZMQExperimental::get_physics_time_step() const +{ + _check_client(); + return _ZMQWrapper::get_sim()->getFloatParam(_ZMQWrapper::get_sim()->floatparam_physicstimestep); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_physics_time_step + * @param time_step + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_physics_time_step(const double &time_step) const +{ + _check_client(); + _ZMQWrapper::get_sim()->setFloatParam(_ZMQWrapper::get_sim()->floatparam_physicstimestep, time_step); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_dynamic_engine + * @param engine + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_engine(const ENGINE &engine) +{ + _check_client(); + _ZMQWrapper::get_sim()->setInt32Param(_ZMQWrapper::get_sim()->intparam_dynamic_engine, engines_.at(engine)); +} + +std::string DQ_CoppeliaSimInterfaceZMQExperimental::get_engine() +{ + switch (_get_engine()){ + case ENGINE::BULLET: + return "BULLET"; + case ENGINE::ODE: + return "ODE"; + case ENGINE::VORTEX: + return "VORTEX"; + case ENGINE::NEWTON: + return "NEWTON"; + case ENGINE::MUJOCO: + return "MUJOCO"; + default: // This line is required in GNU/Linux + _throw_runtime_error("wrong argument"); + } +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_gravity + * @param gravity + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_gravity(const DQ &gravity) +{ + VectorXd gravity_vec = gravity.vec3(); + std::vector g = {gravity_vec(0),gravity_vec(1), gravity_vec(2)}; + _check_client(); + _ZMQWrapper::get_sim()->setArrayParam(_ZMQWrapper::get_sim()->arrayparam_gravity, g); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_gravity + * @return + */ +DQ DQ_CoppeliaSimInterfaceZMQExperimental::get_gravity() const +{ + _check_client(); + std::vector g = _ZMQWrapper::get_sim()->getArrayParam(_ZMQWrapper::get_sim()->arrayparam_gravity); + return DQ(0, g.at(0), g.at(1), g.at(2)); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::load_scene loads a scene from your computer. + * @param path_to_filename the path to the scene. This string must containt + * the file extension. + * + * Example: + * + * load_scene("/Users/juanjqo/git/space_robot/scenes/space_robot.ttt"); + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::load_scene(const std::string &path_to_filename) const +{ + _check_client(); + _ZMQWrapper::get_sim()->loadScene(path_to_filename); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::save_scene saves the current scene. + * @param path_to_filename The path where you want to save the scene including + * the name of the scene and its file extension. + * + * Example: + * + * save_scene("/Users/juanjqo/git/space_robot/scenes/space_robot2.ttt"); + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::save_scene(const std::string &path_to_filename) const +{ + _check_client(); + _ZMQWrapper::get_sim()->saveScene(path_to_filename); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::close_scene closes the current scene. + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::close_scene() const +{ + _check_client(); + _ZMQWrapper::get_sim()->closeScene(); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::remove_child_script_from_object + * The script must be located at objectname/script_name + * @param objectname + * @param script_name + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::remove_child_script_from_object(const std::string &objectname, const std::string &script_name) +{ + _check_client(); + if (object_exist_on_scene(_get_standard_name(objectname)+script_name)) + { + int handle = _get_handle_from_map(_get_standard_name(objectname)+script_name); + _ZMQWrapper::get_sim()->removeObjects({handle}, false); + } +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_object_name(const int &handle, const std::string &new_object_name) const +{ + _check_client(); + _ZMQWrapper::get_sim()->setObjectAlias(handle, new_object_name); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_object_name + * @param current_object_name + * @param new_object_name + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_object_name(const std::string ¤t_object_name, const std::string &new_object_name) +{ + set_object_name(_get_handle_from_map(current_object_name), new_object_name); +} + + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_object_color(const int &handle, + const std::vector &rgba_color) const +{ + _check_client(); + _ZMQWrapper::get_sim()->setShapeColor(handle, "", _ZMQWrapper::get_sim()->colorcomponent_ambient_diffuse, {rgba_color.at(0), rgba_color.at(1),rgba_color.at(2)}); + _ZMQWrapper::get_sim()->setShapeColor(handle, "", _ZMQWrapper::get_sim()->colorcomponent_transparency, {rgba_color.at(3)}); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_object_color(const std::string &objectname, const std::vector& rgba_color) +{ + // For C++20 + // std::string function_name = static_cast(std::source_location::current().function_name()); + std::string function_name = {"DQ_CoppeliaSimInterface::set_object_color"}; + if (rgba_color.size() != 4) + _throw_runtime_error(function_name + ". The rgba_color must be a vector of size 4."); + + set_object_color(_get_handle_from_map(objectname), rgba_color); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_object_as_respondable(const int &handle, const bool &respondable_object) const +{ + _check_client(); + _ZMQWrapper::get_sim()->setObjectInt32Param(handle, + _ZMQWrapper::get_sim()->shapeintparam_respondable, + (respondable_object == true ? 1 : 0)); +} + + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_object_as_respondable(const std::string &objectname, const bool &respondable_object) +{ + set_object_as_respondable(_get_handle_from_map(objectname), respondable_object); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_object_as_static(const int &handle, const bool &static_object) const +{ + _check_client(); + _ZMQWrapper::get_sim()->setObjectInt32Param(handle, + _ZMQWrapper::get_sim()->shapeintparam_static, + (static_object == true ? 1 : 0)); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_object_as_static(const std::string &objectname, const bool &static_object) +{ + set_object_as_static(_get_handle_from_map(objectname), static_object); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_object_parent + * @param handle + * @param parent_handle + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_object_parent(const int &handle, const int &parent_handle, const bool &move_child_to_parent_pose) const +{ + _check_client(); + _ZMQWrapper::get_sim()->setObjectParent(handle, parent_handle, !move_child_to_parent_pose); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_object_parent + * @param objectname + * @param parent_object_name + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_object_parent(const std::string &objectname, + const std::string &parent_object_name, + const bool& move_child_to_parent_pose) +{ + set_object_parent(_get_handle_from_map(objectname), _get_handle_from_map(parent_object_name), move_child_to_parent_pose); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::check_collision + * @param handle1 + * @param handle2 + * @return + */ +bool DQ_CoppeliaSimInterfaceZMQExperimental::check_collision(const int &handle1, const int &handle2) const +{ + _check_client(); + auto [result, collidingObjectHandles] = _ZMQWrapper::get_sim()->checkCollision(handle1, handle2); + return result; +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::check_distance + * @param handle1 + * @param handle2 + * @param threshold + * @return + */ +std::tuple DQ_CoppeliaSimInterfaceZMQExperimental::check_distance(const int &handle1, const int &handle2, const double &threshold) const +{ + _check_client(); + auto [result, data, objectHandlePair] = _ZMQWrapper::get_sim()->checkDistance(handle1, handle2, threshold); + //[obj1X obj1Y obj1Z obj2X obj2Y obj2Z dist] + DQ point1 = DQ(0, data.at(0), data.at(1), data.at(2)); + DQ point2 = DQ(0, data.at(3), data.at(4), data.at(5)); + double distance = data.at(6); + return {distance, point1, point2}; +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::check_distance + * @param objectname1 + * @param objectname2 + * @param threshold + * @return + */ +std::tuple DQ_CoppeliaSimInterfaceZMQExperimental::check_distance(const std::string &objectname1, const std::string &objectname2, const double &threshold) +{ + return check_distance(_get_handle_from_map(objectname1), _get_handle_from_map(objectname2), threshold); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::compute_distance + * @param handle1 + * @param handle2 + * @param threshold + * @return + */ +double DQ_CoppeliaSimInterfaceZMQExperimental::compute_distance(const int &handle1, const int &handle2, const double &threshold) const +{ + return std::get<0>(check_distance(handle1, handle2, threshold)); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::compute_distance + * @param objectname1 + * @param objectname2 + * @param threshold + * @return + */ +double DQ_CoppeliaSimInterfaceZMQExperimental::compute_distance(const std::string &objectname1, const std::string &objectname2, const double &threshold) +{ + return compute_distance(_get_handle_from_map(objectname1), _get_handle_from_map(objectname2), threshold); +} + + + + +void DQ_CoppeliaSimInterfaceZMQExperimental::create_plane(const std::string &name, const std::vector &sizes, const std::vector &rgba_color, const bool &add_normal, const double &normal_scale) const +{ + int primitive_handle = add_primitive(PRIMITIVE::PLANE, name, + {sizes.at(0), sizes.at(1), sizes.at(1)}); + set_object_color(primitive_handle, rgba_color); + set_object_as_respondable(primitive_handle, false); + set_object_as_static(primitive_handle, true); + std::vector children_names; + + if (add_normal) + { + double rfc = 0.02*normal_scale; + std::vector scaled_size = {rfc*sizes.at(0),rfc* sizes.at(1), 0.2*normal_scale*sizes.at(1)}; + _create_static_axis_at_origin(primitive_handle, name, scaled_size, AXIS::k, 1); + } + merge_shapes(primitive_handle); +} + + + +void DQ_CoppeliaSimInterfaceZMQExperimental::create_line(const std::string &name, const std::vector &thickness_and_length, const std::vector &rgba_color, const bool &add_arrow, const double &arrow_scale) const +{ + int primitive_handle = add_primitive(PRIMITIVE::CYLINDER, name, + {thickness_and_length.at(0), thickness_and_length.at(0), thickness_and_length.at(1)}); + set_object_color(primitive_handle, rgba_color); + set_object_as_respondable(primitive_handle, false); + set_object_as_static(primitive_handle, true); + std::vector children_names; + if (add_arrow) + { + // Add the normal + + double rfc = 2*arrow_scale; + std::vector arrow_size = {rfc*thickness_and_length.at(0),rfc*thickness_and_length.at(0), 0.02*arrow_scale*thickness_and_length.at(1)}; + std::string arrow_name = _get_standard_name(name)+std::string("_normal"); + int arrow_handle = add_primitive(PRIMITIVE::CONE, arrow_name, arrow_size); + children_names.push_back(arrow_name); + set_static_object_properties(arrow_handle, + primitive_handle , + 1+0.5*E_*0.5*thickness_and_length.at(1)*k_, + {0,0,1,1}); + } + merge_shapes(primitive_handle); + //_update_created_handles_map(name, children_names); +} + + + + +void DQ_CoppeliaSimInterfaceZMQExperimental::create_cylinder(const std::string &name, const std::vector &width_and_length, const std::vector &rgba_color, const bool &add_line, const double &line_scale) const +{ + int primitive_handle = add_primitive(PRIMITIVE::CYLINDER, name, + {width_and_length.at(0), width_and_length.at(0), width_and_length.at(1)}); + set_object_color(primitive_handle, rgba_color); + set_object_as_respondable(primitive_handle, false); + set_object_as_static(primitive_handle, true); + std::vector children_names; + std::string line_name = _get_standard_name(name)+std::string("_line"); + if (add_line) + { + double wscale = 0.05*line_scale; + double lscale = 1.1*line_scale; + int line_handle = add_primitive(PRIMITIVE::CYLINDER, line_name, + {wscale*width_and_length.at(0), wscale*width_and_length.at(0), lscale*width_and_length.at(1)}); + children_names.push_back(line_name); + set_static_object_properties(line_handle, + primitive_handle, + DQ(1), + {0,0,1,1}); + + double rfc = 2*wscale*line_scale; + std::vector arrow_size = {rfc*width_and_length.at(0),rfc*width_and_length.at(0), 0.02*line_scale*width_and_length.at(1)}; + std::string arrow_name = _get_standard_name(name)+std::string("_normal"); + int arrow_handle = add_primitive(PRIMITIVE::CONE, arrow_name, arrow_size); + set_static_object_properties(arrow_handle, + primitive_handle, + 1+0.5*E_*0.5*lscale*width_and_length.at(1)*k_, + {0,0,1,1}); + children_names.push_back(arrow_name); + + } + merge_shapes(primitive_handle); + //_update_created_handles_map(name, children_names); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::merge_shapes + * @param parent_handle + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::merge_shapes(const int &parent_handle) const +{ + std::vector shapehandles = _ZMQWrapper::get_sim()->getObjectsInTree(parent_handle, //_get_handle_from_map(name), + _ZMQWrapper::get_sim()->object_shape_type, + 0); + std::reverse(shapehandles.begin(), shapehandles.end()); + _ZMQWrapper::get_sim()->groupShapes(shapehandles, false); +} + + + +void DQ_CoppeliaSimInterfaceZMQExperimental::create_reference_frame(const std::string &name, const double &scale, const std::vector &thickness_and_length) const +{ + int primitive_handle = add_primitive(PRIMITIVE::SPHEROID, name, + {1.5*scale*thickness_and_length.at(0), 1.5*scale*thickness_and_length.at(0), 1.5*scale*thickness_and_length.at(0)}); + set_object_color(primitive_handle, {1,1,1,0.5}); + set_object_as_respondable(primitive_handle, false); + set_object_as_static(primitive_handle, true); + std::vector children_names; + std::vector auxdest; + + std::vector scaled_size = {scale*thickness_and_length.at(0),scale*thickness_and_length.at(0), scale*thickness_and_length.at(1)}; + _create_static_axis_at_origin(primitive_handle, name, scaled_size, AXIS::k, 1); + _create_static_axis_at_origin(primitive_handle, name, scaled_size, AXIS::i, 1); + _create_static_axis_at_origin(primitive_handle, name, scaled_size, AXIS::j, 1); + merge_shapes(primitive_handle); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::draw_trajectory + * @param point + * @param size + * @param color + * @param max_item_count + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::draw_permanent_trajectory(const DQ &point, const double &size, const std::vector &color, const int &max_item_count) +{ + _check_client(); + if (!is_pure(point) or !is_quaternion(point)) + { + // For C++20 + // std::string function_name = static_cast(std::source_location::current().function_name()); + std::string function_name = {"DQ_CoppeliaSimInterface::draw_permanent_trajectory"}; + _throw_runtime_error(function_name + ". The point must be a pure quaternion."); + } + VectorXd vpoint = point.vec3(); + std::vector itemdata = {0,0,0,vpoint(0), vpoint(1), vpoint(2)}; + auto drawn_handle = _ZMQWrapper::get_sim()->addDrawingObject(_ZMQWrapper::get_sim()->drawing_lines+_ZMQWrapper::get_sim()->drawing_cyclic,size,0,-1,max_item_count, color); + _ZMQWrapper::get_sim()->addDrawingObjectItem( + drawn_handle, + itemdata + ); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::add_simulation_lua_script + * @param script_name + * @param script_code + * @return + */ +int DQ_CoppeliaSimInterfaceZMQExperimental::add_simulation_lua_script(const std::string &script_name, const std::string& script_code) +{ + _check_client(); + int scriptHandle = _ZMQWrapper::get_sim()->createScript(_ZMQWrapper::get_sim()->scripttype_simulation, + script_code, 0, "lua"); + set_object_name(scriptHandle, _remove_first_slash_from_string(script_name)); + return scriptHandle; +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_bounding_box_size + * @param handle + * @return + */ +std::vector DQ_CoppeliaSimInterfaceZMQExperimental::get_bounding_box_size(const int &handle) const +{ + _check_client(); + auto [size, pose] = _ZMQWrapper::get_sim()->getShapeBB(handle); + return size; +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_bounding_box_size + * @param objectname + * @return + */ +std::vector DQ_CoppeliaSimInterfaceZMQExperimental::get_bounding_box_size(const std::string &objectname) +{ + return get_bounding_box_size(_get_handle_from_map(objectname)); +} + + + + +bool DQ_CoppeliaSimInterfaceZMQExperimental::mujoco_is_used() +{ + return _get_engine() == ENGINE::MUJOCO ? true : false; +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_impratio This attribute determines the ratio of + * frictional-to-normal constraint impedance for elliptic friction cones. + * The setting of solimp determines a single impedance value for + * all contact dimensions, which is then modulated by this attribute. + * Settings larger than 1 cause friction forces to be “harder” than + * normal forces, having the general effect of preventing slip, + * without increasing the actual friction coefficient. + * For pyramidal friction cones the situation is more complex because + * the pyramidal approximation mixes normal and frictional dimensions + * within each basis vector; it is not recommended to use high impratio + * values with pyramidal cones. + * + * @param impratio + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_impratio(const double &impratio) +{ + _ZMQWrapper::get_sim()->setEngineFloatParam(_ZMQWrapper::get_sim()->mujoco_global_impratio,-1, impratio); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_wind Velocity vector of the medium (i.e., wind). + * This vector is subtracted from the 3D translational velocity of each body, + * and the result is used to compute viscous, lift and drag forces acting on the body; + * @param wind + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_wind(const std::vector &wind) +{ + std::vector mujoco_global_param= {_ZMQWrapper::get_sim()->mujoco_global_wind1, + _ZMQWrapper::get_sim()->mujoco_global_wind2, + _ZMQWrapper::get_sim()->mujoco_global_wind3}; + _check_sizes(wind, mujoco_global_param, "Error in DQ_CoppeliaSimInterface::set_mujoco_global_wind: " + "argument must be a vector of size "+std::to_string(mujoco_global_param.size())); + for (size_t i=0;isetEngineFloatParam(mujoco_global_param.at(i),-1, wind.at(i)); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_density Density of the medium, not to be confused with the geom density used to infer masses and inertias. + * This parameter is used to simulate lift and drag forces, which scale quadratically with velocity. + * In SI units the density of air is around 1.2 while the density of water is around 1000 depending on temperature. Setting density to 0 disables lift and drag forces. + * @param density + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_density(const double &density) +{ + _ZMQWrapper::get_sim()->setEngineFloatParam(_ZMQWrapper::get_sim()->mujoco_global_density,-1, density); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_viscosity Viscosity of the medium. This parameter is used to simulate viscous forces, + * which scale linearly with velocity. + * In SI units the viscosity of air is around 0.00002 while the viscosity of water is around 0.0009 depending on temperature. + * Setting viscosity to 0 disables viscous forces. + * Note that the default Euler integrator handles damping in + * the joints implicitly – which improves stability and accuracy. + * It does not presently do this with body viscosity. + * Therefore, if the goal is merely to create a damped simulation + * (as opposed to modeling the specific effects of viscosity), + * we recommend using joint damping rather than body viscosity, + * or switching to the implicit or implicitfast integrators. + * @param viscosity + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_viscosity(const double &viscosity) +{ + _ZMQWrapper::get_sim()->setEngineFloatParam(_ZMQWrapper::get_sim()->mujoco_global_viscosity,-1, viscosity); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_boundmass(const double &boundmass) +{ + _ZMQWrapper::get_sim()->setEngineFloatParam(_ZMQWrapper::get_sim()->mujoco_global_boundmass,-1, boundmass); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_boundinertia(const double &boundinertia) +{ + _ZMQWrapper::get_sim()->setEngineFloatParam(_ZMQWrapper::get_sim()->mujoco_global_boundinertia,-1, boundinertia); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_overridemargin(const double &overridemargin) +{ + _ZMQWrapper::get_sim()->setEngineFloatParam(_ZMQWrapper::get_sim()->mujoco_global_overridemargin,-1, overridemargin); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_overridesolref(const std::vector &overridesolref) +{ + std::vector mujoco_global_param = {_ZMQWrapper::get_sim()->mujoco_global_overridesolref1, + _ZMQWrapper::get_sim()->mujoco_global_overridesolref2}; + _check_sizes(overridesolref, mujoco_global_param, "Error in DQ_CoppeliaSimInterface::set_mujoco_global_overridesolref: " + "argument must be a vector of size "+std::to_string(mujoco_global_param.size())); + for (size_t i=0;isetEngineFloatParam(mujoco_global_param.at(i),-1, overridesolref.at(i)); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_overridesolimp(const std::vector &overridesolimp) +{ + std::vector mujoco_global_param = {_ZMQWrapper::get_sim()->mujoco_global_overridesolimp1, + _ZMQWrapper::get_sim()->mujoco_global_overridesolimp2, + _ZMQWrapper::get_sim()->mujoco_global_overridesolimp3, + _ZMQWrapper::get_sim()->mujoco_global_overridesolimp4, + _ZMQWrapper::get_sim()->mujoco_global_overridesolimp5, + }; + _check_sizes(overridesolimp, mujoco_global_param, "Error in DQ_CoppeliaSimInterface::set_mujoco_global_overridesolimp: " + "argument must be a vector of size "+std::to_string(mujoco_global_param.size())); + for (size_t i=0;isetEngineFloatParam(mujoco_global_param.at(i),-1, overridesolimp.at(i)); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_iterations(const int &iterations) +{ + _ZMQWrapper::get_sim()->setEngineInt32Param(_ZMQWrapper::get_sim()->mujoco_global_iterations,-1, iterations); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_integrator(const int &integrator) +{ + _ZMQWrapper::get_sim()->setEngineInt32Param(_ZMQWrapper::get_sim()->mujoco_global_integrator,-1, integrator); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_solver(const int &solver) +{ + _ZMQWrapper::get_sim()->setEngineInt32Param(_ZMQWrapper::get_sim()->mujoco_global_solver,-1, solver); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_njmax(const int &njmax) +{ + _ZMQWrapper::get_sim()->setEngineInt32Param(_ZMQWrapper::get_sim()->mujoco_global_njmax,-1, njmax); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_nstack(const int &nstack) +{ + _ZMQWrapper::get_sim()->setEngineInt32Param(_ZMQWrapper::get_sim()->mujoco_global_nstack,-1, nstack); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_nconmax(const int &nconmax) +{ + _ZMQWrapper::get_sim()->setEngineInt32Param(_ZMQWrapper::get_sim()->mujoco_global_nconmax,-1, nconmax); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_cone(const int &cone) +{ + _ZMQWrapper::get_sim()->setEngineInt32Param(_ZMQWrapper::get_sim()->mujoco_global_cone,-1, cone); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_overridekin(const int &overridekin) +{ + _ZMQWrapper::get_sim()->setEngineInt32Param(_ZMQWrapper::get_sim()->mujoco_global_overridekin,-1, overridekin); +} + +/* +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_rebuildcondition(const int &rebuildcondition) +{ + sim_->setEngineInt32Param(sim_->mujoco_global_rebuildcondition,-1, rebuildcondition); +} +*/ + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_computeinertias(const bool &computeinertias) +{ + _ZMQWrapper::get_sim()->setEngineBoolParam(_ZMQWrapper::get_sim()->mujoco_global_computeinertias,-1, computeinertias); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_multithreaded(const bool &multithreaded) +{ + _ZMQWrapper::get_sim()->setEngineBoolParam(_ZMQWrapper::get_sim()->mujoco_global_multithreaded,-1, multithreaded); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_multiccd(const bool &multiccd) +{ + _ZMQWrapper::get_sim()->setEngineBoolParam(_ZMQWrapper::get_sim()->mujoco_global_multiccd,-1, multiccd); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_balanceinertias(const bool &balanceinertias) +{ + _ZMQWrapper::get_sim()->setEngineBoolParam(_ZMQWrapper::get_sim()->mujoco_global_balanceinertias,-1, balanceinertias); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_global_overridecontacts(const bool &overridecontacts) +{ + _ZMQWrapper::get_sim()->setEngineBoolParam(_ZMQWrapper::get_sim()->mujoco_global_overridecontacts,-1, overridecontacts); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_joint_stiffness + * Joint stiffness. + * If this value is positive, a spring will be created with equilibrium position + * given by springref below. + * The spring force is computed along with the other passive forces. + * @param jointname + * @param stiffness + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_joint_stiffness(const std::string &jointname, const double &stiffness) +{ + _ZMQWrapper::get_sim()->setEngineFloatParam(_ZMQWrapper::get_sim()->mujoco_joint_stiffness,_get_handle_from_map(jointname), stiffness); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_joint_stiffness + * Joint stiffness. + * If this value is positive, a spring will be created with equilibrium position + * given by springref below. + * The spring force is computed along with the other passive forces. + * @param jointnames + * @param stiffness + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_joint_stiffnesses(const std::vector &jointnames, + const double &stiffness) +{ + for (size_t i=0;isetEngineFloatParam(_ZMQWrapper::get_sim()->mujoco_joint_damping, _get_handle_from_map(jointname), damping); +} + + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_joint_armature Armature inertia (or rotor inertia, or reflected inertia) + * of all degrees of freedom created by this joint. These are constants added to the + * diagonal of the inertia matrix in generalized coordinates. + * They make the simulation more stable, and often increase physical realism. + * This is because when a motor is attached to the system with a transmission + * that amplifies the motor force by c, the inertia of the rotor + * (i.e., the moving part of the motor) is amplified by c*c. + * The same holds for gears in the early stages of planetary gear boxes. + * These extra inertias often dominate the inertias of the robot parts that are + * represented explicitly in the model, and the armature attribute is the way to model them. + * @param jointname + * @param armature + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_joint_armature(const std::string &jointname, const double &armature) +{ + _ZMQWrapper::get_sim()->setEngineFloatParam(_ZMQWrapper::get_sim()->mujoco_joint_armature,_get_handle_from_map(jointname), armature); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_body_friction(const std::string &bodyname, const std::vector &friction) +{ + std::vector mujoco_body_param = {_ZMQWrapper::get_sim()->mujoco_body_friction1, + _ZMQWrapper::get_sim()->mujoco_body_friction2, + _ZMQWrapper::get_sim()->mujoco_body_friction3}; + _check_sizes(friction, mujoco_body_param, "Error in DQ_CoppeliaSimInterface::set_mujoco_body_friction: " + "friction must be a vector of size "+std::to_string(mujoco_body_param.size())); + for (size_t i=0;isetEngineFloatParam(mujoco_body_param.at(i), _get_handle_from_map(bodyname),friction.at(i)); +} + + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_get_mass + * @param handle + * @return + */ +double DQ_CoppeliaSimInterfaceZMQExperimental::get_mass(const int &handle) const +{ + _check_client(); + return _ZMQWrapper::get_sim()->getShapeMass(handle); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_get_mass + * @param object_name + * @return + */ +double DQ_CoppeliaSimInterfaceZMQExperimental::get_mass(const std::string &object_name) +{ + return get_mass(_get_handle_from_map(object_name)); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_get_center_of_mass + * @param handle + * @param reference_frame + * @return + */ +DQ DQ_CoppeliaSimInterfaceZMQExperimental::get_center_of_mass(const int &handle, const REFERENCE &reference_frame) const +{ + DQ COM_body_frame; + MatrixXd Inertia_maxtrix_body_frame; + std::tie(COM_body_frame, Inertia_maxtrix_body_frame) =get_center_of_mass_and_inertia_matrix(handle); + if (reference_frame == REFERENCE::BODY_FRAME) + return COM_body_frame; + else + { + DQ x_0_bodyFrame = _get_object_pose(handle); + DQ x_bodyFrame_com = 1 + 0.5*E_*COM_body_frame; + return (x_0_bodyFrame*x_bodyFrame_com).translation(); + } +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_get_center_of_mass + * @param object_name + * @param reference_frame + * @return + */ +DQ DQ_CoppeliaSimInterfaceZMQExperimental::get_center_of_mass(const std::string &object_name, const REFERENCE &reference_frame) +{ + return get_center_of_mass(_get_handle_from_map(object_name), reference_frame); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_get_inertia_matrix + * @param handle + * @param reference_frame + * @return + */ +MatrixXd DQ_CoppeliaSimInterfaceZMQExperimental::get_inertia_matrix(const int &handle, const REFERENCE &reference_frame) +{ + DQ COM_body_frame; + MatrixXd Inertia_maxtrix_body_frame; + std::tie(COM_body_frame, Inertia_maxtrix_body_frame) =get_center_of_mass_and_inertia_matrix(handle); + if (reference_frame == REFERENCE::BODY_FRAME) + return Inertia_maxtrix_body_frame; + else + { + DQ x_0_bodyFrame = _get_object_pose(handle); + DQ x_bodyFrame_com = 1 + 0.5*E_*COM_body_frame; + DQ x_0_com = x_0_bodyFrame*x_bodyFrame_com; + MatrixXd R_0_COM = get_rotation_matrix(x_0_com.P()); + return R_0_COM*Inertia_maxtrix_body_frame*R_0_COM.transpose(); + } +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::_get_inertia_matrix + * @param link_name + * @param reference_frame + * @return + */ +MatrixXd DQ_CoppeliaSimInterfaceZMQExperimental::get_inertia_matrix(const std::string &link_name, const REFERENCE &reference_frame) +{ + return get_inertia_matrix(_get_handle_from_map(link_name), reference_frame); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_engine returns the current engine in the simulation scene. + * @return + */ +DQ_CoppeliaSimInterfaceZMQExperimental::ENGINE DQ_CoppeliaSimInterfaceZMQExperimental::_get_engine() +{ + _check_client(); + int64_t eng = _ZMQWrapper::get_sim()->getInt32Param(_ZMQWrapper::get_sim()->intparam_dynamic_engine); + return engines_invmap.at(eng); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_velocity_const_params + * @return + */ +std::vector DQ_CoppeliaSimInterfaceZMQExperimental::get_velocity_const_params() const +{ + std::vector params = {_ZMQWrapper::get_sim()->shapefloatparam_init_velocity_a, + _ZMQWrapper::get_sim()->shapefloatparam_init_velocity_b, + _ZMQWrapper::get_sim()->shapefloatparam_init_velocity_g, + _ZMQWrapper::get_sim()->shapefloatparam_init_velocity_x, + _ZMQWrapper::get_sim()->shapefloatparam_init_velocity_y, + _ZMQWrapper::get_sim()->shapefloatparam_init_velocity_z + }; + return params; +} + +std::string DQ_CoppeliaSimInterfaceZMQExperimental::get_resources_path() const +{ + return _ZMQWrapper::get_sim()->getStringParam(_ZMQWrapper::get_sim()->stringparam_resourcesdir); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::load_model + * @param path_to_filename + * @param desired_model_name + * @return + */ +bool DQ_CoppeliaSimInterfaceZMQExperimental::_load_model(const std::string &path_to_filename, + const std::string &desired_model_name, + const bool &remove_child_script) +{ + int rtn = _ZMQWrapper::get_sim()->loadModel(path_to_filename); + if (rtn != -1) + { + set_object_name(rtn, _remove_first_slash_from_string(desired_model_name)); + _get_handle_from_map(_get_standard_name(desired_model_name)); // This is to update the map only. + if (remove_child_script) + { + remove_child_script_from_object(std::string("/") + + _remove_first_slash_from_string(desired_model_name)); + } + return true; + }else{ + return false; + } +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_transformation_matrix + * @param coeff_vector + * @return + */ +MatrixXd DQ_CoppeliaSimInterfaceZMQExperimental::get_transformation_matrix(const std::vector &coeff_vector) const +{ + std::vector coeff = coeff_vector; + MatrixXd TM = Map(coeff.data(), + coeff.size()).reshaped(4,3).transpose(); + return TM; +} + +/** + * @brief This function returns a rotation matrix given a rotation unit quaternion. + * @param r unit quaternion. + * @returns The rotation matrix. + */ +MatrixXd DQ_CoppeliaSimInterfaceZMQExperimental::get_rotation_matrix(const DQ& r) const{ + Matrix R; + VectorXd vecr = r.vec4(); + double w = vecr(0); + double a = vecr(1); + double b = vecr(2); + double c = vecr(3); + R << 1-2*(b*b +c*c), 2*(a*b-w*c), 2*(a*c+w*b), + 2*(a*b+w*c), 1-2*(a*a+c*c), 2*(b*c-w*a), + 2*(a*c-w*b), 2*(b*c+w*a), 1-2*(a*a+b*b); + + return R; +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_rotation_from_direction + * @param direction + * @return + */ +DQ DQ_CoppeliaSimInterfaceZMQExperimental::get_pose_from_direction(const DQ& direction, const DQ& point) +{ + DQ base_direction = k_; + DQ nx; + double argument = std::round(static_cast(dot(base_direction,direction.P()))*100000)/100000; + double phi = acos(argument); + if (phi == 0) + nx = base_direction; + else + nx = cross(base_direction, direction); + DQ r = cos(phi/2) + nx.normalize()*sin(phi/2); + return r + 0.5*E_*point*r; +} + + +std::vector DQ_CoppeliaSimInterfaceZMQExperimental::_create_static_axis_at_origin(const int& parent_handle, + const std::string& parent_name, + const std::vector& sizes, + const AXIS& axis, + const double& alpha_color) const +{ + std::vector created_primitives{}; + std::string name; + DQ dqaxis; + std::vector color; + DQ rotation; + const double angle = M_PI/2; // std::numbers::pi/2; + switch(axis) + { + case AXIS::i: + name = _get_standard_name(parent_name)+std::string("_x"); + dqaxis = i_; + color = {1,0,0,alpha_color}; + rotation = cos(angle/2) + j_*sin(angle/2); + break; + case AXIS::j: + name = _get_standard_name(parent_name)+std::string("_y"); + dqaxis = j_; + color = {0,1,0,alpha_color}; + rotation = cos(-angle/2) + i_*sin(-angle/2); + break; + case AXIS::k: + name = _get_standard_name(parent_name)+std::string("_z"); + dqaxis = k_; + color = {0,0,1,alpha_color}; + rotation = DQ(1); + break; + } + int primitive_handle = add_primitive(PRIMITIVE::CYLINDER,name,sizes); + created_primitives.push_back(name); + set_static_object_properties(primitive_handle, + parent_handle, + rotation+0.5*E_*0.5*sizes.at(2)*dqaxis*rotation, + color + ); + std::vector arrow_size = {2*sizes.at(0), 2*sizes.at(1), 2*sizes.at(1)}; + std::string arrow_name = _get_standard_name(name)+std::string("_"); + int primitive_arrow = add_primitive(PRIMITIVE::CONE, arrow_name, arrow_size); + created_primitives.push_back(arrow_name); + set_static_object_properties(primitive_arrow, + parent_handle , + rotation+0.5*E_*sizes.at(2)*dqaxis*rotation, + color); + return created_primitives; +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_static_object_properties(const std::string &name, + const std::string &parent_name, + const DQ &pose, + const std::vector &rgba_color) +{ + set_object_color(name, rgba_color); + set_object_as_respondable(name, false); + set_object_as_static(name, true); + set_object_pose(name, pose); + set_object_parent(name, parent_name, false); +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_static_object_properties(const int &handle, + const int &parent_handle, + const DQ &pose, + const std::vector &rgba_color) const +{ + set_object_color(handle, rgba_color); + set_object_as_respondable(handle, false); + set_object_as_static(handle, true); + _set_object_pose(handle, pose); + set_object_parent(handle, parent_handle, false); +} + +int DQ_CoppeliaSimInterfaceZMQExperimental::get_primitive_identifier(const PRIMITIVE &primitive) const +{ + switch (primitive) + { + case PRIMITIVE::PLANE: + return _ZMQWrapper::get_sim()->primitiveshape_plane; + case PRIMITIVE::DISC: + return _ZMQWrapper::get_sim()->primitiveshape_disc; + case PRIMITIVE::CUBOID: + return _ZMQWrapper::get_sim()->primitiveshape_cuboid; + case PRIMITIVE::SPHEROID: + return _ZMQWrapper::get_sim()->primitiveshape_spheroid; + case PRIMITIVE::CYLINDER: + return _ZMQWrapper::get_sim()->primitiveshape_cylinder; + case PRIMITIVE::CONE: + return _ZMQWrapper::get_sim()->primitiveshape_cone; + case PRIMITIVE::CAPSULE: + return _ZMQWrapper::get_sim()->primitiveshape_capsule; + default: // This line is required in GNU/Linux + _throw_runtime_error("wrong argument"); + + } +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::get_center_of_mass_and_inertia_matrix + * @param handle + * @return + */ +std::tuple DQ_CoppeliaSimInterfaceZMQExperimental::get_center_of_mass_and_inertia_matrix(const int &handle) const +{ + std::vector inertia_matrix_coeff; + std::vector center_of_mass_coeff; + std::tie(inertia_matrix_coeff, center_of_mass_coeff) = _ZMQWrapper::get_sim()->getShapeInertia(handle); + + MatrixXd Inertia_maxtrix_body_frame = Map(inertia_matrix_coeff.data(), + inertia_matrix_coeff.size()).reshaped(3,3); + MatrixXd COM_body_frame_matrix = get_transformation_matrix(center_of_mass_coeff); + DQ COM_body_frame = DQ(COM_body_frame_matrix.col(3)); + double mass = get_mass(handle); + + return {COM_body_frame, Inertia_maxtrix_body_frame/mass}; +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::plot_reference_frame adds a reference frame object + * in the CoppeliaSim scene. + * @param name The name of the reference frame + * @param pose The position and orientation of the reference frame + * @param scale The scale of the reference frame. + * @param thickness_and_length The thicknessand and length. + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::plot_reference_frame(const std::string &name, + const DQ &pose, + const double &scale, + const std::vector &thickness_and_length) +{ + _check_client(); + + // For C++20 + // std::string function_name = static_cast(std::source_location::current().function_name()); + std::string function_name = {"DQ_CoppeliaSimInterface::plot_reference_frame"}; + + if (!is_unit(pose)) + _throw_runtime_error(function_name + ". The pose must be a unit dual quaternion!"); + + if (thickness_and_length.size() != 2) + _throw_runtime_error(function_name + ". The thickness_and_length must be vector of size 2."); + + if (!object_exist_on_scene(name)) + { + create_reference_frame(name, scale, thickness_and_length); + } + set_object_pose(name, pose); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::plot_plane adds a plane object in the CoppeliaSim scene + * @param name + * @param normal_to_the_plane + * @param location + * @param sizes + * @param rgba_color + * @param add_normal + * @param normal_scale + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::plot_plane(const std::string &name, const DQ &normal_to_the_plane, const DQ &location, const std::vector &sizes, const std::vector &rgba_color, const bool &add_normal, const double &normal_scale) +{ + // For C++20 + // std::string function_name = static_cast(std::source_location::current().function_name()); + std::string function_name = {"DQ_CoppeliaSimInterface::plot_plane"}; + + if (!is_unit(normal_to_the_plane) or !is_quaternion(normal_to_the_plane)) + _throw_runtime_error(function_name + ". The normal to the plane must be a unit quaternion!"); + if (!is_pure(location) or !is_quaternion(location)) + _throw_runtime_error(function_name + ". The location must be a pure quaternion!"); + + if (sizes.size() != 2) + _throw_runtime_error(function_name + ". The sizes must be vector of size 2."); + + if (rgba_color.size() != 4) + _throw_runtime_error(function_name + ". The rgba_color must be vector of size 4."); + + if (!object_exist_on_scene(name)) + { + create_plane(name, sizes, rgba_color, add_normal, normal_scale); + } + set_object_pose(name, get_pose_from_direction(normal_to_the_plane, location)); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::plot_line add a segment line in the CoppeliaSim scene + * @param name + * @param line_direction + * @param location + * @param thickness_and_length + * @param rgba_color + * @param add_arrow + * @param arrow_scale + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::plot_line(const std::string &name, const DQ &line_direction, const DQ &location, const std::vector &thickness_and_length, const std::vector &rgba_color, const bool &add_arrow, const double &arrow_scale) +{ + // For C++20 + // std::string function_name = static_cast(std::source_location::current().function_name()); + std::string function_name = {"DQ_CoppeliaSimInterface::plot_line"}; + + if (!is_unit(line_direction) or !is_quaternion(line_direction)) + _throw_runtime_error(function_name + ". The line direction must be a unit quaternion!"); + + if (!is_pure(location) or !is_quaternion(location)) + _throw_runtime_error(function_name + ". The location must be a pure quaternion!"); + + if (thickness_and_length.size() != 2) + _throw_runtime_error(function_name + ". The thickness_and_length must be vector of size 2."); + + if (rgba_color.size() != 4) + _throw_runtime_error(function_name + ". The rgba_color must be vector of size 4."); + + if (!object_exist_on_scene(name)) + { + create_line(name, thickness_and_length, rgba_color, add_arrow, arrow_scale); + } + set_object_pose(name, get_pose_from_direction(line_direction, location)); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::plot_cylinder adds a cylinder in the CoppeliaSim scene + * @param name + * @param direction + * @param location + * @param width_and_length + * @param rgba_color + * @param add_line + * @param line_scale + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::plot_cylinder(const std::string &name, const DQ &direction, const DQ &location, const std::vector &width_and_length, const std::vector &rgba_color, const bool &add_line, const double &line_scale) +{ + // For C++20 + // std::string function_name = static_cast(std::source_location::current().function_name()); + std::string function_name = {"DQ_CoppeliaSimInterface::plot_cylinder"}; + if (!is_unit(direction) or !is_quaternion(direction)) + _throw_runtime_error(function_name + ". The line direction must be a unit quaternion!"); + + if (!is_pure(location) or !is_quaternion(location)) + _throw_runtime_error(function_name + ". The location must be a pure quaternion!"); + + if (width_and_length.size() != 2) + _throw_runtime_error(function_name + ". The thickness_and_length must be vector of size 2."); + + if (rgba_color.size() != 4) + _throw_runtime_error(function_name + ". The rgba_color must be vector of size 4."); + + if (!object_exist_on_scene(name)) + { + create_cylinder(name, width_and_length, rgba_color, add_line, line_scale); + } + set_object_pose(name, get_pose_from_direction(direction, location)); +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::plot_sphere adds an sphere object in the CoppeliaSim scene + * @param name + * @param location + * @param size + * @param rgba_color + */ +void DQ_CoppeliaSimInterfaceZMQExperimental::plot_sphere(const std::string &name, const DQ &location, const double &size, const std::vector rgba_color) +{ + // For C++20 + // std::string function_name = static_cast(std::source_location::current().function_name()); + std::string function_name = {"DQ_CoppeliaSimInterface::plot_sphere"}; + + if (!is_pure(location) or !is_quaternion(location)) + _throw_runtime_error(function_name + ". The location must be a pure quaternion!"); + if (rgba_color.size() != 4) + _throw_runtime_error(function_name + ". The rgba_color must be vector of size 4."); + + if (!object_exist_on_scene(name)) + { + int primitive_handle = add_primitive(PRIMITIVE::SPHEROID, name,{size, size, size}); + set_object_color(primitive_handle, rgba_color); + set_object_as_respondable(primitive_handle, false); + set_object_as_static(primitive_handle, true); + //std::vector children_names; + //_update_created_handles_map(name, children_names); + } + set_object_pose(name, 1+0.5*E_*location); +} + + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::load_model loads a model to + * the scene. + * + * @param path_to_filename The path to the model. + * @param desired_model_name The name you want for the loaded model. + * @param load_model_only_if_missing If the model exists (with the same alias) + * the model is not loaded. (Default) + * @param remove_child_script Remove the associated child script of the model + * (Default) + * @return A boolean flag. True if the model was loaded. False otherwise. + */ +bool DQ_CoppeliaSimInterfaceZMQExperimental::load_model(const std::string &path_to_filename, + const std::string &desired_model_name, + const bool &load_model_only_if_missing, + const bool &remove_child_script) +{ + if (load_model_only_if_missing == true) + { + if (!object_exist_on_scene(std::string("/") + + _remove_first_slash_from_string(desired_model_name))) + { + return _load_model(path_to_filename, desired_model_name, remove_child_script); + }else + return true; + }else + {// Load the model even if the model is already on the scene + return _load_model(path_to_filename, desired_model_name, remove_child_script); + } +} + +/** + * @brief DQ_CoppeliaSimInterfaceZMQExperimental::::load_from_model_browser loads a model from + * the CoppeliaSim model browser. + * + * Ex: load_from_model_browser("/robots/non-mobile/FrankaEmikaPanda.ttm", "/Franka"); + * + * @param path_to_filename The path to the model relative to the model browser. + * @param desired_model_name The name you want for the loaded model. + * @param load_model_only_if_missing If the model exists (with the same alias) + * the model is not loaded. (Default) + * @param remove_child_script Remove the associated child script of the model + * (Default) + * @return A boolean flag. True if the model was loaded. False otherwise. + */ + +bool DQ_CoppeliaSimInterfaceZMQExperimental::load_from_model_browser(const std::string &path_to_filename, const std::string &desired_model_name, const bool &load_model_only_if_missing, const bool &remove_child_script) +{ + _check_client(); + return load_model(get_resources_path() + std::string("/models") + path_to_filename, + desired_model_name, load_model_only_if_missing, remove_child_script); +} + +int DQ_CoppeliaSimInterfaceZMQExperimental::add_primitive(const PRIMITIVE &primitive, const std::string &name, const std::vector &sizes) const +{ + if (!object_exist_on_scene(name)) + { + _check_client(); + int shapeHandle = _ZMQWrapper::get_sim()->createPrimitiveShape(get_primitive_identifier(primitive), sizes, 0); + set_object_name(shapeHandle, _remove_first_slash_from_string(name)); + return shapeHandle; + }else + return -1; +} + +bool DQ_CoppeliaSimInterfaceZMQExperimental::object_exist_on_scene(const std::string &objectname) const +{ + std::optional options = {{"noError", false}}; + try { + _check_client(); + auto rtn = _ZMQWrapper::get_sim()->getObject(_get_standard_name(objectname), options); + return (rtn != -1) ? true : false; + } catch (...) { + return false; + } +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::remove_object(const std::string &objectname, const bool &remove_children) +{ + _check_client(); + auto standard_objectname = _get_standard_name(objectname); + auto handle = _get_handle_from_map(standard_objectname); + + if (remove_children) + { + auto handles = _ZMQWrapper::get_sim()->getObjectsInTree(handle, _ZMQWrapper::get_sim()->handle_all, 0); + if (handles.size() > 0) + { + auto objectnames = _get_object_names(handles); + _ZMQWrapper::get_sim()->removeObjects(handles, false); + for (std::size_t i=0; iremoveObjects({handle}, false); + _update_map(standard_objectname, handle, UPDATE_MAP::REMOVE); + } +} + +void DQ_CoppeliaSimInterfaceZMQExperimental::draw_trajectory(const std::string &objectname, const double &size, const std::vector &rgb_color, const int &max_item_count) +{ + // For C++20 + // std::string function_name = static_cast(std::source_location::current().function_name()); + std::string function_name = {"DQ_CoppeliaSimInterface::draw_trajectory"}; + if (!object_exist_on_scene(objectname)) + _throw_runtime_error(function_name + ". The object " +objectname+ " is not on the scene."); + + if (!object_exist_on_scene(objectname+"/drawer")) + { + if (rgb_color.size() != 3) + _throw_runtime_error(function_name + ". The rgb_color must be vector of size 3."); + int r = rgb_color.at(0); + int g = rgb_color.at(1); + int b = rgb_color.at(2); + + std::string setting_str = " dr=sim.addDrawingObject(sim.drawing_lines|sim.drawing_cyclic,"+std::to_string(size)+",0,-1,"+std::to_string(max_item_count)+",{"+std::to_string(r)+","+std::to_string(g)+","+std::to_string(b)+"})" + "\n"; + + std::string code = + "function sysCall_init()" + std::string("\n") + + " h=sim.getObjectHandle(sim.handle_self)" + "\n" + + setting_str + + " pt=sim.getObjectPosition(h,-1) " + "\n" + + "end" + "\n" + + "function sysCall_sensing()" + "\n" + + " local l={pt[1],pt[2],pt[3]} " + "\n" + + " pt=sim.getObjectPosition(h,-1)" + "\n" + + " l[4]=pt[1]" + "\n" + + " l[5]=pt[2]" + "\n" + + " l[6]=pt[3]" + "\n" + + " sim.addDrawingObjectItem(dr,l)" + "\n" + + "end "; + add_simulation_lua_script("/drawer", code); + set_object_parent("/drawer", objectname); + } +} + +bool DQ_CoppeliaSimInterfaceZMQExperimental::check_collision(const std::string &objectname1, const std::string &objectname2) +{ + return check_collision(_get_handle_from_map(objectname1), _get_handle_from_map(objectname2)); +} + + +void DQ_CoppeliaSimInterfaceZMQExperimental::set_mujoco_joint_dampings(const std::vector &jointnames, + const double &damping) +{ + for (size_t i=0;i &jointnames, const double &armature) +{ + for (size_t i=0;i &bodynames, const std::vector &friction) +{ + for (auto& bodyname : bodynames) + set_mujoco_body_friction(bodyname, friction); +} + + + + diff --git a/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimModels.cpp b/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimModels.cpp deleted file mode 100644 index d62b4a4..0000000 --- a/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimModels.cpp +++ /dev/null @@ -1,176 +0,0 @@ -#include - -namespace DQ_robotics -{ -/** - * @brief DQ_CoppeliaSimModels::_get_interface_sptr - * @return - */ -std::shared_ptr DQ_CoppeliaSimModels::_get_interface_sptr() const -{ - return coppeliasim_interface_sptr_; -} - -/** - * @brief DQ_CoppeliaSimModels::_get_string_from_others - * @param model - * @return - */ -std::string DQ_CoppeliaSimModels::_get_string_from_others(const COMPONENTS &model) -{ - switch (model) - { - case COMPONENTS::REFERENCE_FRAME: - return std::string("/other/reference frame.ttm"); - case COMPONENTS::ARUCO_MARKER: - return std::string("/other/aruco marker.ttm"); - case COMPONENTS::PIONEER_P3DX: - return std::string("/robots/mobile/pioneer p3dx.ttm"); - case COMPONENTS::GPS: - return std::string("/components/sensors/GPS.ttm"); - case COMPONENTS::FRANKA_EMIKA_PANDA: - return std::string("/robots/non-mobile/FrankaEmikaPanda.ttm"); - case COMPONENTS::JACO: - return std::string("/robots/non-mobile/Jaco arm.ttm"); - case COMPONENTS::UR3: - return std::string("/robots/non-mobile/UR3.ttm"); - case COMPONENTS::UR5: - return std::string("/robots/non-mobile/UR5.ttm"); - case COMPONENTS::UR10: - return std::string("/robots/non-mobile/UR10.ttm"); - default: // This line is required in GNU/Linux - return std::string(""); - } -} - -/** - * @brief DQ_CoppeliaSimModels::DQ_CoppeliaSimModels - * @param coppeliasim_interface_sptr - */ -DQ_CoppeliaSimModels::DQ_CoppeliaSimModels(const std::shared_ptr &coppeliasim_interface_sptr) - :coppeliasim_interface_sptr_(coppeliasim_interface_sptr) -{ - -} - -/** - * @brief DQ_CoppeliaSimModels::_load_model - * @param model - * @param desired_model_name - * @param load_model_only_if_missing - * @param remove_child_script - */ -void DQ_CoppeliaSimModels::_load_model(const COMPONENTS &model, - const std::string &desired_model_name, - const bool &load_model_only_if_missing, - const bool &remove_child_script) -{ - _get_interface_sptr()->load_from_model_browser(_get_string_from_others(model), - desired_model_name, - load_model_only_if_missing, - remove_child_script); - -} - -/** - * @brief DQ_CoppeliaSimModels::load_model - * @param model - * @param desired_model_name - * @param pose - * @param load_model_only_if_missing - * @param remove_child_script - */ -void DQ_CoppeliaSimModels::load_model(const COMPONENTS &model, - const std::string &desired_model_name, - const DQ &pose, - const bool &load_model_only_if_missing, - const bool &remove_child_script) -{ - _load_model(model, desired_model_name, load_model_only_if_missing, remove_child_script); - if (pose != DQ(0)) - _get_interface_sptr()->set_object_pose(desired_model_name, pose); -} - - -/** - * @brief DQ_CoppeliaSimModels::load_reference_frame - * @param desired_model_name - * @param pose - * @param load_model_only_if_missing - * @param remove_child_script - */ -void DQ_CoppeliaSimModels::load_reference_frame(const std::string &desired_model_name, - const DQ &pose, - const bool &load_model_only_if_missing, - const bool &remove_child_script) -{ - _load_model(COMPONENTS::REFERENCE_FRAME, desired_model_name, load_model_only_if_missing, remove_child_script); - if (pose != DQ(0)) - _get_interface_sptr()->set_object_pose(desired_model_name, pose); -} - -/** - * @brief DQ_CoppeliaSimModels::load_reference_frames - * @param desired_model_names - * @param poses - */ -void DQ_CoppeliaSimModels::load_reference_frames(const std::vector &desired_model_names, const std::vector &poses) -{ - size_t n = desired_model_names.size(); - for (size_t i=0;i &desired_model_names) -{ - for (auto& name : desired_model_names) - load_reference_frame(name, DQ(1), true, true); -} - -/** - * @brief DQ_CoppeliaSimModels::load_panda - * @param desired_model_name - * @param pose - */ -void DQ_CoppeliaSimModels::load_panda(const std::string &desired_model_name, - const DQ &pose) { - load_model(COMPONENTS::FRANKA_EMIKA_PANDA, desired_model_name, pose, true, - true); -} - -/** - * @brief DQ_CoppeliaSimModels::load_primitive - * @param primitive - * @param name - * @param pose - * @param sizes - * @param rgb_color - * @param transparency - * @param set_as_static - * @param set_as_respondable - */ -void DQ_CoppeliaSimModels::load_primitive(const DQ_CoppeliaSimInterface::PRIMITIVE &primitive, - const std::string &name, - const DQ &pose, - const std::vector sizes, - const std::vector rgba_color, - const bool &set_as_static, - const bool &set_as_respondable) -{ - if (!_get_interface_sptr()->object_exist_on_scene(name)) - { - _get_interface_sptr()->add_primitive(primitive, name, sizes); - _get_interface_sptr()->set_object_color(name, rgba_color); - _get_interface_sptr()->set_object_as_respondable(name, set_as_respondable); - _get_interface_sptr()->set_object_as_static(name, set_as_static); - _get_interface_sptr()->set_object_pose(name, pose); - } -} - - -} diff --git a/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.cpp b/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.cpp index 8b6e1ce..8fa2329 100644 --- a/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.cpp +++ b/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.cpp @@ -1,7 +1,7 @@ /** (C) Copyright 2024 DQ Robotics Developers -This file is based on DQ Robotics. +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 @@ -16,40 +16,26 @@ This file is based on DQ Robotics. 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: -- Juan Jose Quiroz Omana - - Responsible for the original implementation. - The DQ_CoppeliaSimRobot class is partially based on the DQ_VrepRobot class - (https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_VrepRobot.h) + 1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) + - Responsible for the original implementation. This class is based on + https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_SerialVrepRobot.h */ #include + namespace DQ_robotics { -/** - * @brief DQ_CoppeliaSimRobot::_get_interface_sptr - * @return - */ -std::shared_ptr DQ_CoppeliaSimRobot::_get_interface_sptr() const +DQ_CoppeliaSimRobot::DQ_CoppeliaSimRobot(const std::string &robot_name) + :robot_name_{robot_name} { - return coppeliasim_interface_sptr_; -} -/** - * @brief DQ_CoppeliaSimRobot::DQ_CoppeliaSimRobot - * @param robot_name - * @param coppeliasim_interface_sptr - */ -DQ_CoppeliaSimRobot::DQ_CoppeliaSimRobot(const std::string& robot_name, - const std::shared_ptr& coppeliasim_interface_sptr) -{ - robot_name_ = robot_name; - if(!coppeliasim_interface_sptr) - throw std::runtime_error("Null reference to coppeliasim_interface, initialize it first!"); - coppeliasim_interface_sptr_ = coppeliasim_interface_sptr; } } + diff --git a/src/dqrobotics/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.cpp b/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.cpp similarity index 55% rename from src/dqrobotics/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.cpp rename to src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.cpp index 2c46195..f9fa673 100644 --- a/src/dqrobotics/interfaces/coppeliasim/DQ_SerialCoppeliaSimRobot.cpp +++ b/src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.cpp @@ -19,23 +19,22 @@ This file is based on DQ Robotics. Contributors: - Juan Jose Quiroz Omana - Responsible for the original implementation. - The DQ_SerialCoppeliaSimRobot class is partially based on the DQ_SerialVrepRobot class + The DQ_CoppeliaSimRobotZMQ class is partially based on the DQ_SerialVrepRobot class (https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_SerialVrepRobot.h) */ -#include +#include namespace DQ_robotics { -/** - * @brief DQ_SerialCoppeliaSimRobot::_initialize_jointnames_from_coppeliasim - */ -void DQ_SerialCoppeliaSimRobot::_initialize_jointnames_from_coppeliasim() + + +void DQ_CoppeliaSimRobotZMQ::_initialize_jointnames_from_coppeliasim() { - jointnames_ = _get_interface_sptr()->get_jointnames_from_parent_object(robot_name_); + jointnames_ = _get_exp_interface_sptr()->get_jointnames_from_parent_object(robot_name_); base_frame_name_ = jointnames_.at(0); } @@ -43,29 +42,43 @@ void DQ_SerialCoppeliaSimRobot::_initialize_jointnames_from_coppeliasim() /** * @brief DQ_SerialCoppeliaSimRobot::DQ_SerialCoppeliaSimRobot * @param robot_name - * @param coppeliasim_interface_sptr + * @param interface_sptr */ -DQ_SerialCoppeliaSimRobot::DQ_SerialCoppeliaSimRobot(const std::string &robot_name, - const std::shared_ptr &coppeliasim_interface_sptr) - :DQ_CoppeliaSimRobot(robot_name, coppeliasim_interface_sptr) +DQ_CoppeliaSimRobotZMQ::DQ_CoppeliaSimRobotZMQ(const std::string &robot_name, + const std::shared_ptr &interface_sptr) + :DQ_CoppeliaSimRobot(robot_name), interface_sptr_{interface_sptr} { + coppeliasim_interface_sptr_ = std::make_shared(interface_sptr_); _initialize_jointnames_from_coppeliasim(); // By Default, the robot is controlled by joint positions with both the dynamic engine // and the stepping mode enabled. - joint_control_mode_ = DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE::POSITION; + joint_control_mode_ = DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::POSITION; robot_is_used_as_visualization_tool_ = false; } +std::shared_ptr DQ_CoppeliaSimRobotZMQ::_get_interface_sptr() +{ + return interface_sptr_; +} + +/** + * @brief DQ_SerialCoppeliaSimRobot::_initialize_jointnames_from_coppeliasim + */ +std::shared_ptr DQ_CoppeliaSimRobotZMQ::_get_exp_interface_sptr() +{ + return coppeliasim_interface_sptr_; +} + /** * @brief DQ_SerialCoppeliaSimRobot::set_operation_modes * @param joint_mode * @param joint_control_mode */ -void DQ_SerialCoppeliaSimRobot::set_operation_modes(const DQ_CoppeliaSimInterface::JOINT_MODE &joint_mode, const DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE &joint_control_mode) +void DQ_CoppeliaSimRobotZMQ::_set_operation_modes(const DQ_CoppeliaSimInterfaceZMQ::JOINT_MODE &joint_mode, const DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE &joint_control_mode) { joint_control_mode_ = joint_control_mode; - _get_interface_sptr()->set_joint_modes(jointnames_, joint_mode); - _get_interface_sptr()->set_joint_control_modes(jointnames_, joint_control_mode); + _get_exp_interface_sptr()->set_joint_modes(jointnames_, joint_mode); + _get_exp_interface_sptr()->set_joint_control_modes(jointnames_, joint_control_mode); } /** @@ -75,12 +88,12 @@ void DQ_SerialCoppeliaSimRobot::set_operation_modes(const DQ_CoppeliaSimInterfac * by joint position commands without taking into account the dynamics. * In other words, the CoppeliaSim scene is used as a visualization tool. */ -void DQ_SerialCoppeliaSimRobot::set_robot_as_visualization_tool() +void DQ_CoppeliaSimRobotZMQ::_set_robot_as_visualization_tool() { _get_interface_sptr()->set_stepping_mode(false); - _get_interface_sptr()->enable_dynamics(false); - _get_interface_sptr()->set_joint_modes(jointnames_, - DQ_CoppeliaSimInterface::JOINT_MODE::KINEMATIC); + _get_exp_interface_sptr()->enable_dynamics(false); + _get_exp_interface_sptr()->set_joint_modes(jointnames_, + DQ_CoppeliaSimInterfaceZMQ::JOINT_MODE::KINEMATIC); robot_is_used_as_visualization_tool_ = true; } @@ -92,11 +105,11 @@ void DQ_SerialCoppeliaSimRobot::set_robot_as_visualization_tool() * * @param joint_control_mode Use POSITION, VELOCITY or TORQUE. */ -void DQ_SerialCoppeliaSimRobot::set_robot_as_dynamic_tool(const DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE &joint_control_mode) +void DQ_CoppeliaSimRobotZMQ::_set_robot_as_dynamic_tool(const DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE &joint_control_mode) { - _get_interface_sptr()->enable_dynamics(true); + _get_exp_interface_sptr()->enable_dynamics(true); _get_interface_sptr()->set_stepping_mode(true); - set_joint_control_type(joint_control_mode); + _set_joint_control_type(joint_control_mode); } /** @@ -104,9 +117,9 @@ void DQ_SerialCoppeliaSimRobot::set_robot_as_dynamic_tool(const DQ_CoppeliaSimIn * of the robot. * @param joint_control_mode Use POSITION, VELOCITY or TORQUE. */ -void DQ_SerialCoppeliaSimRobot::set_joint_control_type(const DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE &joint_control_mode) +void DQ_CoppeliaSimRobotZMQ::_set_joint_control_type(const DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE &joint_control_mode) { - set_operation_modes(DQ_CoppeliaSimInterface::JOINT_MODE::DYNAMIC, joint_control_mode); + _set_operation_modes(DQ_CoppeliaSimInterfaceZMQ::JOINT_MODE::DYNAMIC, joint_control_mode); } /** @@ -114,7 +127,7 @@ void DQ_SerialCoppeliaSimRobot::set_joint_control_type(const DQ_CoppeliaSimInter * * @param u joint positions, velocities or torques. */ -void DQ_SerialCoppeliaSimRobot::set_control_inputs(const VectorXd &u) +void DQ_CoppeliaSimRobotZMQ::_set_control_inputs(const VectorXd &u) { if (robot_is_used_as_visualization_tool_) _get_interface_sptr()->set_joint_positions(jointnames_, u); @@ -122,21 +135,21 @@ void DQ_SerialCoppeliaSimRobot::set_control_inputs(const VectorXd &u) { switch (joint_control_mode_) { - case DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE::FREE: + case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::FREE: break; - case DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE::FORCE: + case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::FORCE: break; - case DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE::VELOCITY: + case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::VELOCITY: _get_interface_sptr()->set_joint_target_velocities(jointnames_, u); break; - case DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE::POSITION: + case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::POSITION: _get_interface_sptr()->set_joint_target_positions(jointnames_, u); break; - case DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE::SPRING: + case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::SPRING: break; - case DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE::CUSTOM: + case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::CUSTOM: break; - case DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE::TORQUE: + case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::TORQUE: _get_interface_sptr()->set_joint_torques(jointnames_, u); break; } @@ -144,12 +157,7 @@ void DQ_SerialCoppeliaSimRobot::set_control_inputs(const VectorXd &u) } } - -/** - * @brief DQ_SerialCoppeliaSimRobot::get_joint_names - * @return - */ -std::vector DQ_SerialCoppeliaSimRobot::get_joint_names() +std::vector DQ_CoppeliaSimRobotZMQ::get_joint_names() { return jointnames_; } @@ -159,7 +167,7 @@ std::vector DQ_SerialCoppeliaSimRobot::get_joint_names() * @brief DQ_SerialCoppeliaSimRobot::set_configuration_space_positions * @param q */ -void DQ_SerialCoppeliaSimRobot::set_configuration_space_positions(const VectorXd &q) +void DQ_CoppeliaSimRobotZMQ::set_configuration_space(const VectorXd &q) { _get_interface_sptr()->set_joint_positions(jointnames_,q); } @@ -169,7 +177,7 @@ void DQ_SerialCoppeliaSimRobot::set_configuration_space_positions(const VectorXd * @brief DQ_SerialCoppeliaSimRobot::get_configuration_space_positions * @return */ -VectorXd DQ_SerialCoppeliaSimRobot::get_configuration_space_positions() +VectorXd DQ_CoppeliaSimRobotZMQ::get_configuration_space() { return _get_interface_sptr()->get_joint_positions(jointnames_); } @@ -179,7 +187,7 @@ VectorXd DQ_SerialCoppeliaSimRobot::get_configuration_space_positions() * @brief DQ_SerialCoppeliaSimRobot::set_target_configuration_space_positions * @param q_target */ -void DQ_SerialCoppeliaSimRobot::set_target_configuration_space_positions(const VectorXd &q_target) +void DQ_CoppeliaSimRobotZMQ::set_target_configuration_space(const VectorXd &q_target) { _get_interface_sptr()->set_joint_target_positions(jointnames_, q_target); } @@ -189,7 +197,7 @@ void DQ_SerialCoppeliaSimRobot::set_target_configuration_space_positions(const V * @brief DQ_SerialCoppeliaSimRobot::get_configuration_space_velocities * @return */ -VectorXd DQ_SerialCoppeliaSimRobot::get_configuration_space_velocities() +VectorXd DQ_CoppeliaSimRobotZMQ::get_configuration_space_velocities() { return _get_interface_sptr()->get_joint_velocities(jointnames_); } @@ -198,7 +206,7 @@ VectorXd DQ_SerialCoppeliaSimRobot::get_configuration_space_velocities() * @brief DQ_SerialCoppeliaSimRobot::set_target_configuration_space_velocities * @param v_target */ -void DQ_SerialCoppeliaSimRobot::set_target_configuration_space_velocities(const VectorXd &v_target) +void DQ_CoppeliaSimRobotZMQ::set_target_configuration_space_velocities(const VectorXd &v_target) { _get_interface_sptr()->set_joint_target_velocities(jointnames_, v_target); } @@ -207,7 +215,7 @@ void DQ_SerialCoppeliaSimRobot::set_target_configuration_space_velocities(const * @brief DQ_SerialCoppeliaSimRobot::set_configuration_space_torques * @param torques */ -void DQ_SerialCoppeliaSimRobot::set_configuration_space_torques(const VectorXd &torques) +void DQ_CoppeliaSimRobotZMQ::set_configuration_space_torques(const VectorXd &torques) { _get_interface_sptr()->set_joint_torques(jointnames_, torques); } @@ -216,9 +224,34 @@ void DQ_SerialCoppeliaSimRobot::set_configuration_space_torques(const VectorXd & * @brief DQ_SerialCoppeliaSimRobot::get_configuration_space_torques * @return */ -VectorXd DQ_SerialCoppeliaSimRobot::get_configuration_space_torques() +VectorXd DQ_CoppeliaSimRobotZMQ::get_configuration_space_torques() { return _get_interface_sptr()->get_joint_torques(jointnames_); } +void DQ_CoppeliaSimRobotZMQ::send_q_to_vrep(const VectorXd &q) +{ + DQ_CoppeliaSimRobotZMQ::set_configuration_space(q); +} + +VectorXd DQ_CoppeliaSimRobotZMQ::get_q_from_vrep() +{ + return DQ_CoppeliaSimRobotZMQ::get_configuration_space(); +} + +void DQ_CoppeliaSimRobotZMQ::set_configuration_space_positions(const VectorXd &q) +{ + DQ_CoppeliaSimRobotZMQ::set_configuration_space(q); +} + +VectorXd DQ_CoppeliaSimRobotZMQ::get_configuration_space_positions() +{ + return DQ_CoppeliaSimRobotZMQ::get_configuration_space(); +} + +void DQ_CoppeliaSimRobotZMQ::set_target_configuration_space_positions(const VectorXd &q_target) +{ + DQ_CoppeliaSimRobotZMQ::set_target_configuration_space(q_target); +} + } diff --git a/src/dqrobotics/interfaces/coppeliasim/internal/_zmq_wrapper.cpp b/src/dqrobotics/interfaces/coppeliasim/internal/_zmq_wrapper.cpp new file mode 100644 index 0000000..ce5a8c2 --- /dev/null +++ b/src/dqrobotics/interfaces/coppeliasim/internal/_zmq_wrapper.cpp @@ -0,0 +1,19 @@ +#include "_zmq_wrapper.h" + +std::shared_ptr _ZMQWrapper::client_; +std::shared_ptr _ZMQWrapper::sim_; + +bool _ZMQWrapper::create_client(const std::string &host, const int &rpcPort, const int &cntPort, const int &verbose_) +{ + if(!client_) + client_ = std::make_shared(host, rpcPort, cntPort, verbose_); + if(!sim_) + sim_ = std::make_shared(client_->getObject().sim()); + + return true; +} + +std::shared_ptr _ZMQWrapper::get_sim() +{ + return sim_; +} diff --git a/src/dqrobotics/interfaces/coppeliasim/internal/_zmq_wrapper.h b/src/dqrobotics/interfaces/coppeliasim/internal/_zmq_wrapper.h new file mode 100644 index 0000000..e9de2e6 --- /dev/null +++ b/src/dqrobotics/interfaces/coppeliasim/internal/_zmq_wrapper.h @@ -0,0 +1,25 @@ +#pragma once +#include + +/** + * @brief The _ZMQWrapper class + * In this initial implementation, each process can only have one live connection with ZMQ. + * More connections will require this singleton implementation to be changed to a map to accomodate it. + * + * Timeout will be added later + */ +class _ZMQWrapper +{ +private: + static std::shared_ptr client_; + static std::shared_ptr sim_; +public: + + static bool create_client(const std::string& host, + const int& rpcPort, + const int& cntPort = -1, + const int& verbose_ = -1); + + static std::shared_ptr get_sim(); + +}; diff --git a/src/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimRobot.cpp b/src/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.cpp similarity index 76% rename from src/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimRobot.cpp rename to src/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.cpp index 8e37fbc..4a84524 100644 --- a/src/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimRobot.cpp +++ b/src/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.cpp @@ -24,7 +24,7 @@ This file is based on DQ Robotics. */ -#include +#include #include namespace DQ_robotics @@ -33,21 +33,21 @@ namespace DQ_robotics /** - * @brief FrankaEmikaPandaCoppeliaSimRobot::FrankaEmikaPandaCoppeliaSimRobot + * @brief FrankaEmikaPandaCoppeliaSimZMQRobot::FrankaEmikaPandaCoppeliaSimZMQRobot * @param robot_name * @param coppeliasim_interface_sptr */ -FrankaEmikaPandaCoppeliaSimRobot::FrankaEmikaPandaCoppeliaSimRobot(const std::string &robot_name, const std::shared_ptr &coppeliasim_interface_sptr) - :DQ_SerialCoppeliaSimRobot(robot_name, coppeliasim_interface_sptr) +FrankaEmikaPandaCoppeliaSimZMQRobot::FrankaEmikaPandaCoppeliaSimZMQRobot(const std::string &robot_name, const std::shared_ptr &coppeliasim_interface_sptr) + :DQ_CoppeliaSimRobotZMQ(robot_name, coppeliasim_interface_sptr) { } /** - * @brief FrankaEmikaPandaCoppeliaSimRobot::kinematics + * @brief FrankaEmikaPandaCoppeliaSimZMQRobot::kinematics * @return */ -DQ_SerialManipulatorMDH FrankaEmikaPandaCoppeliaSimRobot::kinematics() +DQ_SerialManipulatorMDH FrankaEmikaPandaCoppeliaSimZMQRobot::kinematics() { DQ_SerialManipulatorMDH kin = FrankaEmikaPandaRobot::kinematics(); kin.set_reference_frame(_get_interface_sptr()->get_object_pose(base_frame_name_)); diff --git a/src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimRobot.cpp b/src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.cpp similarity index 84% rename from src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimRobot.cpp rename to src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.cpp index de56fdc..3bcace5 100644 --- a/src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimRobot.cpp +++ b/src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.cpp @@ -21,13 +21,13 @@ This file is based on DQ Robotics. - Responsible for the original implementation. */ -#include +#include namespace DQ_robotics { -MatrixXd _get_dh_matrix(const URXCoppeliaSimRobot::MODEL& model); +MatrixXd _get_dh_matrix(const URXCoppeliaSimZMQRobot::MODEL& model); @@ -48,11 +48,11 @@ MatrixXd _get_dh_matrix(const URXCoppeliaSimRobot::MODEL& model); * * @return MatrixXd raw_dh_matrix a matrix related to the D-H parameters */ -MatrixXd _get_dh_matrix(const URXCoppeliaSimRobot::MODEL& model) +MatrixXd _get_dh_matrix(const URXCoppeliaSimZMQRobot::MODEL& model) { const double pi = M_PI; switch (model){ - case URXCoppeliaSimRobot::MODEL::UR5: + case URXCoppeliaSimZMQRobot::MODEL::UR5: { Matrix raw_dh_matrix(5,6); raw_dh_matrix << -pi/2, -pi/2, 0, -pi/2, 0, 0, @@ -69,16 +69,16 @@ MatrixXd _get_dh_matrix(const URXCoppeliaSimRobot::MODEL& model) } } -URXCoppeliaSimRobot::URXCoppeliaSimRobot(const std::string &robot_name, - const std::shared_ptr &coppeliasim_interface_sptr, +URXCoppeliaSimZMQRobot::URXCoppeliaSimZMQRobot(const std::string &robot_name, + const std::shared_ptr &coppeliasim_interface_sptr, const MODEL &model) - :DQ_SerialCoppeliaSimRobot(robot_name, coppeliasim_interface_sptr), model_(model) + :DQ_CoppeliaSimRobotZMQ(robot_name, coppeliasim_interface_sptr), model_(model) { } -DQ_SerialManipulatorDH URXCoppeliaSimRobot::kinematics() +DQ_SerialManipulatorDH URXCoppeliaSimZMQRobot::kinematics() { auto kin = DQ_SerialManipulatorDH(_get_dh_matrix(model_)); kin.set_reference_frame(_get_interface_sptr()->get_object_pose(base_frame_name_)); diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index a83745b..0000000 --- a/src/main.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "RemoteAPIClient.h" -#include - - -int main() -{ - RemoteAPIClient client; - auto sim = client.getObject().sim(); - try - { - sim.startSimulation(); - auto handle = sim.getObject("/Franka"); - std::vector scripthandles = sim.getObjectsInTree(handle, - sim.object_script_type, - 0); - std::cout<<"Number of scripts: "< -#include - -int main() -{ - try - { - DQ_CoppeliaSimInterface vi; - vi.connect("localhost", 23000); - vi.set_stepping_mode(true); - vi.set_engine(DQ_CoppeliaSimInterface::ENGINE::MUJOCO); - - //vi.set_mujoco_global_impratio(1); - //vi.set_mujoco_global_wind({0,0,0}); - //vi.set_mujoco_global_overridesolimp({1,2,3,4,5}); - //vi.set_gravity(DQ(0)); - //vi.set_mujoco_joint_stiffness("Revolute_joint", 0.45); - //vi.set_mujoco_joint_armature("Revolute_joint", 0.1); - - vi.set_mujoco_body_friction("RR_calf_respondable", {0.4, 0.005, 0.0001}); - - std::cout<<" Engine: "< -#include -#include -#include - -using namespace DQ_robotics; -using namespace Eigen; - -int main() -{ - auto vi = std::make_shared(); - vi->connect("localhost", 23000); - vi->close_scene(); - - - vi->plot_reference_frame("/x", DQ(1)); - vi->plot_plane("/plane", k_, 0.5*k_); - vi->plot_line("/line", i_, -0.3*j_); - vi->plot_cylinder("/cyl", i_, 0.1*k_); - - - - vi->start_simulation(); - - - - - - vi->stop_simulation(); - -} diff --git a/src/test_panda.cpp b/src/test_panda.cpp deleted file mode 100644 index 789c9d6..0000000 --- a/src/test_panda.cpp +++ /dev/null @@ -1,54 +0,0 @@ -#include "dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterface.h" - -#include -#include -#include -#include - -using namespace DQ_robotics; -using namespace Eigen; - -int main() -{ - - auto vi1 = std::make_shared(); - - try { - vi1->connect("localhost", 23000, 200);// - vi1->close_scene(); - vi1->load_from_model_browser("/robots/non-mobile/FrankaEmikaPanda.ttm","/Franka", true, false); - //vi1->remove_child_script_from_object("/Franka"); - vi1->draw_trajectory("/Franka/connection", 4, {1,0,0}, 100); - vi1->plot_reference_frame("/x", DQ(1)); - - vi1->plot_plane("/plane", k_, k_,{0.2,0.2}, {1,0,0,0.5}, true); - vi1->plot_line("/line", k_, k_); - vi1->plot_sphere("/mysphere", 1*k_); - vi1->plot_cylinder("/cylinder", k_,0.8*k_, {0.5,1.5}, {1,0,0,0.5}, true, 1); - - //vi1->remove_object("/Sphere", true); - vi1->remove_plotted_object("/x"); - vi1->remove_plotted_object("/plane"); - vi1->remove_plotted_object("/line"); - vi1->remove_plotted_object("/mysphere"); - vi1->remove_plotted_object("/cylinder"); - //auto size_ = vi1->get_bounding_box_size("/line"); - //std::cout<show_map(); - vi1->show_created_handle_map(); - - vi1->start_simulation(); - - - - - - vi1->stop_simulation(); - } catch (const std::runtime_error& e) - { - std::cerr< - - -using namespace DQ_robotics; -using namespace Eigen; - -namespace My{ - class InterfaceUnitTests : public testing::Test { - - protected: - enum class ROBOT_MODE{KINEMATIC, POSITION, VELOCITY, TORQUE}; - std::unique_ptr vi_; - VectorXd q_panda_home_ = (VectorXd(7)<<0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4).finished(); - InterfaceUnitTests() { - vi_ = std::make_unique(); - vi_->connect("localhost", 23000, 1000); - } - - ~InterfaceUnitTests() override { - vi_->stop_simulation(); - delay(); - vi_->close_scene(); - } - - std::vector load_panda(const ROBOT_MODE& robot_mode){ - vi_->load_from_model_browser("/robots/non-mobile/FrankaEmikaPanda.ttm","/Franka", true, true); - auto jointnames = vi_->get_jointnames_from_parent_object("/Franka"); - switch (robot_mode){ - - case ROBOT_MODE::KINEMATIC: - vi_->set_joint_modes(jointnames, DQ_CoppeliaSimInterface::JOINT_MODE::KINEMATIC); - vi_->enable_dynamics(false); - break; - case ROBOT_MODE::POSITION: - vi_->set_joint_modes(jointnames, DQ_CoppeliaSimInterface::JOINT_MODE::DYNAMIC); - vi_->set_joint_control_modes(jointnames, DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE::POSITION); - vi_->enable_dynamics(true); - break; - case ROBOT_MODE::VELOCITY: - vi_->set_joint_modes(jointnames, DQ_CoppeliaSimInterface::JOINT_MODE::DYNAMIC); - vi_->set_joint_control_modes(jointnames, DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE::VELOCITY); - vi_->enable_dynamics(true); - break; - case ROBOT_MODE::TORQUE: - vi_->set_joint_modes(jointnames, DQ_CoppeliaSimInterface::JOINT_MODE::DYNAMIC); - vi_->set_joint_control_modes(jointnames, DQ_CoppeliaSimInterface::JOINT_CONTROL_MODE::TORQUE); - vi_->enable_dynamics(true); - break; - } - return jointnames; - } - - void delay(const int& time_ms = 100){ - std::this_thread::sleep_for(std::chrono::milliseconds(time_ms)); - } - - void SetUp() override { - // Code here will be called immediately after the constructor (right - // before each test). - vi_->close_scene(); - } - - void TearDown() override { - // Code here will be called immediately after each test (right - // before the destructor). - vi_->stop_simulation(); - } - - }; // class InterfaceUnitTests - - - - //----------------------TESTS HERE---------------------------------------- - TEST_F(InterfaceUnitTests, start_stop_simulation){ - vi_->start_simulation(); - delay(); - EXPECT_NE(vi_->get_simulation_state(),8); - EXPECT_NE(vi_->get_simulation_state(),0); - vi_->stop_simulation(); - delay(); - EXPECT_EQ(vi_->get_simulation_state(),0)<<"Error in start_simulation() or stop_simulation()"; - }; - - TEST_F(InterfaceUnitTests, stepping_mode){ - vi_->set_stepping_mode(true); - double T{vi_->get_simulation_time_step()}; - double t{0}; - double elapsed_time{0}; - vi_->start_simulation(); - for (auto i=0; i<5;i++) - { - t = i*T; - elapsed_time = vi_->get_simulation_time(); - vi_->trigger_next_simulation_step(); - } - vi_->set_stepping_mode(false); - EXPECT_EQ(t, elapsed_time)<<"There is something wrong with the stepping mode!"; - - }; - - TEST_F(InterfaceUnitTests, get_object_pose) { - DQ p = -0.1*k_; - DQ x = 1 + 0.5*E_*p; - VectorXd vx = x.vec8(); - VectorXd vxr = vi_->get_object_pose("/Floor").vec8(); - for (auto i=0;iplot_reference_frame("/x", x); - EXPECT_EQ(vi_->get_object_pose("/x"), x)<<"Error in plot_reference_frame()"; - } - - TEST_F(InterfaceUnitTests, plot_plane) { - DQ p = 0.5*i_ + 0.4*j_ + 0.9*k_; - vi_->plot_plane("/plane", k_, p); - EXPECT_EQ(vi_->get_object_translation("/plane"), p)<<"Error in plot_plane()"; - } - - TEST_F(InterfaceUnitTests, plot_line) { - DQ p = 0.5*i_ + 0.4*j_ + 0.9*k_; - vi_->plot_line("/line", k_, p); - EXPECT_EQ(vi_->get_object_translation("/line"), p)<<"Error in plot_line()"; - } - - TEST_F(InterfaceUnitTests, plot_cylinder) { - DQ p = 0.5*i_ + 0.4*j_ + 0.9*k_; - vi_->plot_line("/cylinder", i_, p); - EXPECT_EQ(vi_->get_object_translation("/cylinder"), p)<<"Error in plot_cylinder()"; - } - - - TEST_F(InterfaceUnitTests, plot_sphere) { - DQ p = 0.5*i_ + 0.4*j_ + 0.9*k_; - vi_->plot_sphere("/sphere", p); - EXPECT_EQ(vi_->get_object_translation("/sphere"), p)<<"Error in plot_sphere()"; - } - - TEST_F(InterfaceUnitTests, object_exist) { - vi_->plot_reference_frame("/x", DQ(1)); - EXPECT_EQ(vi_->object_exist_on_scene("/x"), true)<<"Error in object_exist()"; - } - - TEST_F(InterfaceUnitTests, remove_object) { - vi_->plot_reference_frame("/x", DQ(1)); - vi_->remove_object("/x"); - EXPECT_EQ(vi_->object_exist_on_scene("/x"), false)<<"Error in remove_object()"; - } - - TEST_F(InterfaceUnitTests, joint_positions){ - auto jointnames = load_panda(ROBOT_MODE::KINEMATIC); - vi_->set_joint_positions(jointnames, q_panda_home_); - delay(); - auto qr = vi_->get_joint_positions(jointnames); - for (auto i=0;ienable_dynamics(true); - } - - TEST_F(InterfaceUnitTests, target_joint_positions){ - auto jointnames = load_panda(ROBOT_MODE::POSITION); - vi_->start_simulation(); - vi_->set_joint_target_positions(jointnames, q_panda_home_); - delay(800); - auto qr = vi_->get_joint_positions(jointnames); - for (auto i=0;i