Robot arm works slowly with a stagnant moving (dynamixel_workbench)

edit

Hello everyone,

I have got a problem with a robot arm using three dynamixel MX-64AT motors. The motors are controlled with dynamixel_workbench (0.1.4). For this the normal position control .launch, .h and .cpp files were configured. A custom made movement.cpp program reads positions and writes them on the motors. For starting I open a launch file that opens the position_contol.launch and after that I start a second launch file that opens the movement.cpp and starts moving the motors. The problem is that the motors stop at each position, so the movement is very slow and stagnant. Is it possible to get a smooth motion of the motors? Here the code of the selfmade movement.cpp:

#include <ros/ros.h>
#include <std_msgs/Float64.h>
//#include "std_msgs/String.h"
#include <iostream>
#include <fstream>

#include "/home/antrobot/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_controllers/include/dynamixel_workbench_controllers/position_control.h"
//#include <sstream>
using namespace std;

int main(int argc, char **argv)
{

   std_msgs::Float64 msg1;
   std_msgs::Float64 msg2;
   std_msgs::Float64 msg3;
   ifstream joint1;
   ifstream joint2;
   ifstream joint3;

  ros::init(argc, argv, "position_control");


  dynamixel_workbench_msgs::JointCommand joint_command;
  ros::NodeHandle node_handle;

  ros::ServiceClient joint_command_client =               node_handle.serviceClient<dynamixel_workbench_msgs::JointCommand>("/antrobotv3_movement/joint_command");

  if (argc != 2)
  {
    ROS_ERROR("rosrun dynamixel_workbench_operator joint_operator [mode] [pan_pos] [tilt_pos] [coxa_pos]");
ROS_INFO("%i argc",argc);
    return 1;
  }


 while (ros::ok())
  { 

   joint1.open("/home/antrobot/catkin_ws/src/antrobotv3/Daten/joint_1.txt");
   joint2.open("/home/antrobot/catkin_ws/src/antrobotv3/Daten/joint_3.txt");
   joint3.open("/home/antrobot/catkin_ws/src/antrobotv3/Daten/joint_4.txt");

int count = 0;
    while ((joint1 >> msg1.data)
    && (joint2 >> msg2.data)
    && (joint3 >> msg3.data)
     )
    {   

  joint_command.request.unit = argv[1];
  joint_command.request.pan_pos =  msg2.data;
  joint_command.request.tilt_pos = msg3.data;
  joint_command.request.coxa_pos = msg1.data;

//ROS_INFO("hier %f", msg2.data);
  if (joint_command_client.call(joint_command))
  {
    ROS_INFO("[pan_pos: %.2f (value)] [tilt_pos: %.2f (value)] [coxa_pos: %.2f (value)]", joint_command.response.pan_pos, joint_command.response.tilt_pos, joint_command.response.coxa_pos);
  }
  else
  {
    ROS_ERROR("Failed to call service /joint_command");
  }

 }
}
  return 0;
}

I know that it is bad style to link the whole paths, but it did not work in any other.