Added a new class to test the vrep communication in robots commanded by joints velocities

include/robot_coppelia_ros_interface.h

@@ -21,7 +21,9 @@ struct RobotCoppeliaROSConfiguration
     int thread_sampling_time_nsec;
     int port;
     std::string ip;
-
+    std::vector<std::string> 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<DQ_VrepInterface> 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<std::string>& jointnames,
                               const RobotCoppeliaROSConfiguration& configuration,
                               std::atomic_bool* kill_this_node);
 
     RobotCoppeliaRosInterface()=delete;
 
     ~RobotCoppeliaRosInterface();
-    VectorXd  get_joint_positions() const;
+    //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);
-    void set_joint_target_velocities(const VectorXd& target_joint_velocities);
+
+    //void set_joint_target_positions_(const VectorXd& target_joint_positions);
     int control_loop();
 };

src/robot_coppelia_ros_interface.cpp

@@ -10,29 +10,26 @@
  */
 RobotCoppeliaRosInterface::RobotCoppeliaRosInterface(const ros::NodeHandle& nh,
                                                      const std::string& topic_prefix,
-                                                     const std::vector<std::string>& 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_target_joint_positions_  = nh_.subscribe(topic_prefix_ + "/set/target_joint_positions", 1,  &RobotCoppeliaRosInterface::_callback_target_joint_positions, this);
-    //subscriber_joint_state_ = nh_.subscribe(topic_prefix_+ "/get/joint_states", 1,
+    subscriber_target_joint_velocities_ = nh_.subscribe(topic_prefix_ + "/set/target_joint_velocities", 1, &RobotCoppeliaRosInterface::_callback_target_joint_velocities, this);
-    //                                        &RobotCoppeliaRosInterface::_joint_states_callback, this);
-
-    //publisher_target_joint_positions_ = nh_.advertise<std_msgs::Float64MultiArray>
-    //                                    (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);
     publisher_joint_states_ = nh_.advertise<sensor_msgs::JointState>(topic_prefix_+ "/get/joint_states", 1);
 
+    ROS_INFO_STREAM(ros::this_node::getName() << "::Connecting with CoppeliaSim...");
     vi_ = std::make_shared<DQ_VrepInterface>();
     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_);
+                    }
+                //}
             }
-
+            else if (robot_mode_  == std::string("PositionControl"))
-            //auto q = get_joint_positions();
+                {
-            std::cout<<"q: "<<q.transpose()<<std::endl;
+                    if (target_joint_positions_.size()>0)
-            std::cout<<"target q: "<<target_joint_positions_.transpose()<<std::endl;
+                    {
-
+                        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);
+}

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<std::string> 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
     {