From 51910be06d3ac45d7ace18dcf42bff330d33308d Mon Sep 17 00:00:00 2001 From: Juancho Date: Thu, 3 Aug 2023 11:53:57 +0900 Subject: [PATCH] Added a new class to test the vrep communication in robots commanded by joints velocities --- include/robot_coppelia_ros_interface.h | 20 +++++-- src/robot_coppelia_ros_interface.cpp | 70 ++++++++++++----------- src/robot_coppelia_ros_interface_node.cpp | 16 +++++- 3 files changed, 66 insertions(+), 40 deletions(-) diff --git a/include/robot_coppelia_ros_interface.h b/include/robot_coppelia_ros_interface.h index 4edbe9c..9b22f67 100644 --- a/include/robot_coppelia_ros_interface.h +++ b/include/robot_coppelia_ros_interface.h @@ -21,7 +21,9 @@ struct RobotCoppeliaROSConfiguration int thread_sampling_time_nsec; int port; std::string ip; - + std::vector jointnames; + std::string robot_mode; + bool mirror_mode; }; class RobotCoppeliaRosInterface @@ -31,13 +33,18 @@ private: bool _should_shutdown() const; sas::Clock clock_; RobotCoppeliaROSConfiguration configuration_; + std::string robot_mode_ = std::string("VelocityControl"); // PositionControl + bool mirror_mode_ = false; + double gain_ = 0.5; + protected: ros::NodeHandle nh_; VectorXd joint_positions_; VectorXd target_joint_positions_; + VectorXd target_joint_velocities_; std::string topic_prefix_; - ros::Publisher publisher_target_joint_positions_; + //ros::Publisher publisher_target_joint_positions_; ros::Publisher publisher_joint_states_; //ros::Subscriber subscriber_joint_state_; ros::Subscriber subscriber_target_joint_positions_; @@ -47,6 +54,7 @@ protected: std::shared_ptr vi_; void _joint_states_callback(const sensor_msgs::JointState::ConstPtr& jointstate); void _callback_target_joint_positions(const std_msgs::Float64MultiArrayConstPtr &msg); + void _callback_target_joint_velocities(const std_msgs::Float64MultiArrayConstPtr& msg); void send_joint_states(const VectorXd &joint_positions, const VectorXd &joint_velocities, const VectorXd &joint_forces); @@ -54,15 +62,15 @@ protected: public: RobotCoppeliaRosInterface(const ros::NodeHandle& nh, const std::string& topic_prefix, - const std::vector& jointnames, const RobotCoppeliaROSConfiguration& configuration, std::atomic_bool* kill_this_node); RobotCoppeliaRosInterface()=delete; ~RobotCoppeliaRosInterface(); - VectorXd get_joint_positions() const; - void set_joint_target_positions(const VectorXd& target_joint_positions); - void set_joint_target_velocities(const VectorXd& target_joint_velocities); + //VectorXd get_joint_positions() const; + //void set_joint_target_positions(const VectorXd& target_joint_positions); + + //void set_joint_target_positions_(const VectorXd& target_joint_positions); int control_loop(); }; diff --git a/src/robot_coppelia_ros_interface.cpp b/src/robot_coppelia_ros_interface.cpp index 10afcf6..26f4c4c 100644 --- a/src/robot_coppelia_ros_interface.cpp +++ b/src/robot_coppelia_ros_interface.cpp @@ -10,29 +10,26 @@ */ RobotCoppeliaRosInterface::RobotCoppeliaRosInterface(const ros::NodeHandle& nh, const std::string& topic_prefix, - const std::vector& jointnames, const RobotCoppeliaROSConfiguration& configuration, std::atomic_bool* kill_this_node) :nh_(nh), topic_prefix_(topic_prefix), - jointnames_(jointnames), kill_this_node_(kill_this_node), configuration_(configuration), + robot_mode_ (configuration.robot_mode), + jointnames_(configuration.jointnames), + mirror_mode_(configuration.mirror_mode), clock_(configuration.thread_sampling_time_nsec) { - - //subscriber_joint_state_ = nh_.subscribe(topic_prefix_+ "/get/joint_states", 1, - // &RobotCoppeliaRosInterface::_joint_states_callback, this); - - //publisher_target_joint_positions_ = nh_.advertise - // (topic_prefix_ + "/set/target_joint_positions", 1); - - subscriber_target_joint_positions_ = nh_.subscribe(topic_prefix_ + "/set/target_joint_positions", 1, &RobotCoppeliaRosInterface::_callback_target_joint_positions, this); + subscriber_target_joint_positions_ = nh_.subscribe(topic_prefix_ + "/set/target_joint_positions", 1, &RobotCoppeliaRosInterface::_callback_target_joint_positions, this); + subscriber_target_joint_velocities_ = nh_.subscribe(topic_prefix_ + "/set/target_joint_velocities", 1, &RobotCoppeliaRosInterface::_callback_target_joint_velocities, this); publisher_joint_states_ = nh_.advertise(topic_prefix_+ "/get/joint_states", 1); + ROS_INFO_STREAM(ros::this_node::getName() << "::Connecting with CoppeliaSim..."); vi_ = std::make_shared(); vi_->connect(configuration_.ip, configuration_.port, 500, 10); vi_->start_simulation(); + ROS_INFO_STREAM(ros::this_node::getName() << "::Connection ok!"); } RobotCoppeliaRosInterface::~RobotCoppeliaRosInterface() @@ -40,20 +37,14 @@ RobotCoppeliaRosInterface::~RobotCoppeliaRosInterface() vi_->disconnect(); } -/** - * @brief get_joint_positions - * @return - */ +/* VectorXd RobotCoppeliaRosInterface::get_joint_positions() const { return joint_positions_; } -/** - * @brief set_joint_target_positions - * @param target_joint_positions - */ +/* void RobotCoppeliaRosInterface::set_joint_target_positions(const VectorXd &target_joint_positions) { @@ -62,12 +53,7 @@ void RobotCoppeliaRosInterface::set_joint_target_positions(const VectorXd &targe publisher_target_joint_positions_.publish(ros_msg); } - -void RobotCoppeliaRosInterface::set_joint_target_velocities(const VectorXd &target_joint_velocities) -{ - -} - + */ void RobotCoppeliaRosInterface::send_joint_states(const VectorXd &joint_positions, const VectorXd &joint_velocities, const VectorXd &joint_forces) { @@ -85,7 +71,7 @@ int RobotCoppeliaRosInterface::control_loop() { try{ clock_.init(); - ROS_INFO_STREAM(ros::this_node::getName() << "::Waiting to connect with CoppeliaSim..."); + ROS_INFO_STREAM(ros::this_node::getName() << "::Starting control loop..."); while(not _should_shutdown()) @@ -97,16 +83,29 @@ int RobotCoppeliaRosInterface::control_loop() joint_positions_ = q; send_joint_states(q, VectorXd(), VectorXd()); - if (target_joint_positions_.size()>0) + if (robot_mode_ == std::string("VelocityControl")) { - vi_->set_joint_target_positions(jointnames_, target_joint_positions_); + //if (mirror_mode_ == true) + //{ + if (target_joint_positions_.size()>0) + { + vi_->set_joint_target_velocities(jointnames_, gain_*(target_joint_positions_-q)); + } + //} + //else{ + if (target_joint_velocities_.size()>0) + { + vi_->set_joint_target_velocities(jointnames_, target_joint_velocities_); + } + //} } - - //auto q = get_joint_positions(); - std::cout<<"q: "<0) + { + vi_->set_joint_target_positions(jointnames_, target_joint_positions_); + } + } ros::spinOnce(); } @@ -143,3 +142,8 @@ void RobotCoppeliaRosInterface::_callback_target_joint_positions(const std_msgs: { target_joint_positions_ = sas::std_vector_double_to_vectorxd(msg->data); } + +void RobotCoppeliaRosInterface::_callback_target_joint_velocities(const std_msgs::Float64MultiArrayConstPtr &msg) +{ + target_joint_velocities_ = sas::std_vector_double_to_vectorxd(msg->data); +} diff --git a/src/robot_coppelia_ros_interface_node.cpp b/src/robot_coppelia_ros_interface_node.cpp index 6d72b5d..ced3976 100644 --- a/src/robot_coppelia_ros_interface_node.cpp +++ b/src/robot_coppelia_ros_interface_node.cpp @@ -84,12 +84,26 @@ int main(int argc, char **argv) //4000000 RobotCoppeliaROSConfiguration configuration; + + sas::get_ros_param(nh,"/vrep_ip", configuration.ip); + sas::get_ros_param(nh,"/robot_mode", configuration.robot_mode); + sas::get_ros_param(nh,"/thread_sampling_time_nsec",configuration.thread_sampling_time_nsec); + sas::get_ros_param(nh,"/vrep_port", configuration.port); + sas::get_ros_param(nh,"/vrep_robot_joint_names", configuration.jointnames); + sas::get_ros_param(nh,"/mirror_mode", configuration.mirror_mode); + + /* configuration.thread_sampling_time_nsec = 4000000; configuration.ip = "10.198.113.159"; configuration.port = 20021; std::vector jointnames = {"Franka_joint1","Franka_joint2","Franka_joint3", "Franka_joint4","Franka_joint5","Franka_joint6","Franka_joint7"}; - RobotCoppeliaRosInterface robot_coppelia_ros_interface(nh, "/franka_1_sim",jointnames, configuration, &kill_this_process); + configuration.robot_mode = std::string("VelocityControl"); + configuration.mirror_mode = true; + + configuration.jointnames = jointnames; + */ + RobotCoppeliaRosInterface robot_coppelia_ros_interface(nh, "/franka_1_sim", configuration, &kill_this_process); try {