Skip to content

Fix DQ_CoppeliaSimRobotZMQ class and its derived robots #7

New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

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

Fix DQ_CoppeliaSimRobotZMQ class and its derived robots #7

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
f7932e1
[DQ_CoppeliaSimRobotZMQ] Fixed broken references to DQ_CoppeliaSimInt…
ffasilva Jan 28, 2025
58d6604
[URXCoppeliaSimZMQRobot] Fixed broken references to DQ_CoppeliaSimInt…
ffasilva Jan 28, 2025
dc625a6
[FrankaEmikaPandaCoppeliaSimZMQRobot] Fixed broken references to DQ_C…
ffasilva Jan 28, 2025
afdb321
[CMakeLists] Uncommented the inclusion of the 'DQ_CoppeliaSimRobotZMQ…
ffasilva Jan 28, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 11 additions & 11 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,10 @@ add_library(${PROJECT_NAME} SHARED
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
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

)
Expand Down Expand Up @@ -70,13 +70,13 @@ endif()
INSTALL(FILES
include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.h
include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQExperimental.h
# include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.h
include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.h
DESTINATION "include/dqrobotics/interfaces/coppeliasim")

# Robots
INSTALL(FILES
# include/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.h
# include/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.h
include/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.h
include/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.h
DESTINATION "include/dqrobotics/interfaces/coppeliasim/robots")

################################################################
Expand All @@ -87,14 +87,14 @@ INSTALL(FILES
INSTALL(FILES
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
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/FrankaEmikaPandaCoppeliaSimZMQRobot.cpp
# src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.cpp
src/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.cpp
src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.cpp
DESTINATION "src/dqrobotics/interfaces/coppeliasim/robots")


22 changes: 12 additions & 10 deletions include/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,33 +27,35 @@ This file is based on DQ Robotics.
#pragma once
#include <vector>
#include <dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQ.h>
#include <dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimInterfaceZMQExperimental.h>
#include <dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobot.h>

#include<memory>

namespace DQ_robotics
{
class DQ_CoppeliaSimRobotZMQ: public DQ_CoppeliaSimRobot
{
private:
std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ> interface_sptr_;
std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ::experimental> coppeliasim_interface_sptr_;
DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE joint_control_mode_;
std::shared_ptr<DQ_CoppeliaSimInterfaceZMQExperimental> coppeliasim_interface_sptr_;
DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE joint_control_mode_;
bool robot_is_used_as_visualization_tool_;

std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ::experimental> _get_exp_interface_sptr();
std::shared_ptr<DQ_CoppeliaSimInterfaceZMQExperimental> _get_exp_interface_sptr();

void _initialize_jointnames_from_coppeliasim();

protected:
std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ> _get_interface_sptr();
std::shared_ptr<DQ_CoppeliaSimInterfaceZMQExperimental> _get_interface_sptr();

DQ_CoppeliaSimRobotZMQ(const std::string& robot_name,
const std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ>& interface_sptr);
const std::shared_ptr<DQ_CoppeliaSimInterfaceZMQExperimental>& coppeliasim_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_operation_modes(const DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_MODE& joint_mode,
const DQ_CoppeliaSimInterfaceZMQExperimental::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_robot_as_dynamic_tool(const DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE& joint_control_mode);
void _set_joint_control_type(const DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE& joint_control_mode);
void _set_control_inputs(const VectorXd& u);

public:
Expand Down
2 changes: 1 addition & 1 deletion include/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ class FrankaEmikaPandaCoppeliaSimZMQRobot: public DQ_CoppeliaSimRobotZMQ
{
public:
FrankaEmikaPandaCoppeliaSimZMQRobot(const std::string& robot_name,
const std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ>& coppeliasim_interface_sptr
const std::shared_ptr<DQ_CoppeliaSimInterfaceZMQExperimental>& coppeliasim_interface_sptr
);
DQ_SerialManipulatorMDH kinematics();
};
Expand Down
4 changes: 2 additions & 2 deletions include/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,8 @@ class URXCoppeliaSimZMQRobot: public DQ_CoppeliaSimRobotZMQ

public:
URXCoppeliaSimZMQRobot(const std::string& robot_name,
const std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ>& coppeliasim_interface_sptr,
const MODEL& model = MODEL::UR5);
const std::shared_ptr<DQ_CoppeliaSimInterfaceZMQExperimental>& coppeliasim_interface_sptr,
const MODEL& model = MODEL::UR5);
DQ_SerialManipulatorDH kinematics();
private:
URXCoppeliaSimZMQRobot::MODEL model_;
Expand Down
38 changes: 19 additions & 19 deletions src/dqrobotics/interfaces/coppeliasim/DQ_CoppeliaSimRobotZMQ.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,26 +45,26 @@ void DQ_CoppeliaSimRobotZMQ::_initialize_jointnames_from_coppeliasim()
* @param interface_sptr
*/
DQ_CoppeliaSimRobotZMQ::DQ_CoppeliaSimRobotZMQ(const std::string &robot_name,
const std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ> &interface_sptr)
:DQ_CoppeliaSimRobot(robot_name), interface_sptr_{interface_sptr}
const std::shared_ptr<DQ_CoppeliaSimInterfaceZMQExperimental>& coppeliasim_interface_sptr)
:DQ_CoppeliaSimRobot(robot_name)
{
coppeliasim_interface_sptr_ = std::make_shared<DQ_CoppeliaSimInterfaceZMQ::experimental>(interface_sptr_);
coppeliasim_interface_sptr_ = coppeliasim_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_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::POSITION;
joint_control_mode_ = DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE::POSITION;
robot_is_used_as_visualization_tool_ = false;
}

std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ> DQ_CoppeliaSimRobotZMQ::_get_interface_sptr()
std::shared_ptr<DQ_CoppeliaSimInterfaceZMQExperimental> DQ_CoppeliaSimRobotZMQ::_get_interface_sptr()
{
return interface_sptr_;
return coppeliasim_interface_sptr_;
}

/**
* @brief DQ_SerialCoppeliaSimRobot::_initialize_jointnames_from_coppeliasim
*/
std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ::experimental> DQ_CoppeliaSimRobotZMQ::_get_exp_interface_sptr()
std::shared_ptr<DQ_CoppeliaSimInterfaceZMQExperimental> DQ_CoppeliaSimRobotZMQ::_get_exp_interface_sptr()
{
return coppeliasim_interface_sptr_;
}
Expand All @@ -74,7 +74,7 @@ std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ::experimental> DQ_CoppeliaSimRobotZMQ
* @param joint_mode
* @param 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)
void DQ_CoppeliaSimRobotZMQ::_set_operation_modes(const DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_MODE &joint_mode, const DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE &joint_control_mode)
{
joint_control_mode_ = joint_control_mode;
_get_exp_interface_sptr()->set_joint_modes(jointnames_, joint_mode);
Expand All @@ -93,7 +93,7 @@ void DQ_CoppeliaSimRobotZMQ::_set_robot_as_visualization_tool()
_get_interface_sptr()->set_stepping_mode(false);
_get_exp_interface_sptr()->enable_dynamics(false);
_get_exp_interface_sptr()->set_joint_modes(jointnames_,
DQ_CoppeliaSimInterfaceZMQ::JOINT_MODE::KINEMATIC);
DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_MODE::KINEMATIC);
robot_is_used_as_visualization_tool_ = true;
}

Expand All @@ -105,7 +105,7 @@ void DQ_CoppeliaSimRobotZMQ::_set_robot_as_visualization_tool()
*
* @param joint_control_mode Use POSITION, VELOCITY or TORQUE.
*/
void DQ_CoppeliaSimRobotZMQ::_set_robot_as_dynamic_tool(const DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE &joint_control_mode)
void DQ_CoppeliaSimRobotZMQ::_set_robot_as_dynamic_tool(const DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE &joint_control_mode)
{
_get_exp_interface_sptr()->enable_dynamics(true);
_get_interface_sptr()->set_stepping_mode(true);
Expand All @@ -117,9 +117,9 @@ void DQ_CoppeliaSimRobotZMQ::_set_robot_as_dynamic_tool(const DQ_CoppeliaSimInte
* of the robot.
* @param joint_control_mode Use POSITION, VELOCITY or TORQUE.
*/
void DQ_CoppeliaSimRobotZMQ::_set_joint_control_type(const DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE &joint_control_mode)
void DQ_CoppeliaSimRobotZMQ::_set_joint_control_type(const DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE &joint_control_mode)
{
_set_operation_modes(DQ_CoppeliaSimInterfaceZMQ::JOINT_MODE::DYNAMIC, joint_control_mode);
_set_operation_modes(DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_MODE::DYNAMIC, joint_control_mode);
}

/**
Expand All @@ -135,21 +135,21 @@ void DQ_CoppeliaSimRobotZMQ::_set_control_inputs(const VectorXd &u)
{
switch (joint_control_mode_)
{
case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::FREE:
case DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE::FREE:
break;
case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::FORCE:
case DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE::FORCE:
break;
case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::VELOCITY:
case DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE::VELOCITY:
_get_interface_sptr()->set_joint_target_velocities(jointnames_, u);
break;
case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::POSITION:
case DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE::POSITION:
_get_interface_sptr()->set_joint_target_positions(jointnames_, u);
break;
case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::SPRING:
case DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE::SPRING:
break;
case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::CUSTOM:
case DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE::CUSTOM:
break;
case DQ_CoppeliaSimInterfaceZMQ::JOINT_CONTROL_MODE::TORQUE:
case DQ_CoppeliaSimInterfaceZMQExperimental::JOINT_CONTROL_MODE::TORQUE:
_get_interface_sptr()->set_joint_torques(jointnames_, u);
break;
}
Expand Down
2 changes: 1 addition & 1 deletion src/dqrobotics/interfaces/coppeliasim/robots/FrankaEmikaPandaCoppeliaSimZMQRobot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ namespace DQ_robotics
* @param robot_name
* @param coppeliasim_interface_sptr
*/
FrankaEmikaPandaCoppeliaSimZMQRobot::FrankaEmikaPandaCoppeliaSimZMQRobot(const std::string &robot_name, const std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ> &coppeliasim_interface_sptr)
FrankaEmikaPandaCoppeliaSimZMQRobot::FrankaEmikaPandaCoppeliaSimZMQRobot(const std::string &robot_name, const std::shared_ptr<DQ_CoppeliaSimInterfaceZMQExperimental> &coppeliasim_interface_sptr)
:DQ_CoppeliaSimRobotZMQ(robot_name, coppeliasim_interface_sptr)
{

Expand Down
4 changes: 2 additions & 2 deletions src/dqrobotics/interfaces/coppeliasim/robots/URXCoppeliaSimZMQRobot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,8 +70,8 @@ MatrixXd _get_dh_matrix(const URXCoppeliaSimZMQRobot::MODEL& model)
}

URXCoppeliaSimZMQRobot::URXCoppeliaSimZMQRobot(const std::string &robot_name,
const std::shared_ptr<DQ_CoppeliaSimInterfaceZMQ> &coppeliasim_interface_sptr,
const MODEL &model)
const std::shared_ptr<DQ_CoppeliaSimInterfaceZMQExperimental> &coppeliasim_interface_sptr,
const MODEL &model)
:DQ_CoppeliaSimRobotZMQ(robot_name, coppeliasim_interface_sptr), model_(model)
{

Expand Down