Dynamixel 2.0 - Execute Joint Trajectories

edit

I was able to figure it out for trajectory_msgs/JointTrajectory by using the following:

  ROS_INFO("Joint trajectory consists of %ld points.", jointTrajectory.points.size());
  ros::Time trajectoryStartTime = ros::Time::now();
  for (int i = 0; i < jointTrajectory.points.size(); i++)
  {
    while ( (ros::Time::now() - trajectoryStartTime).toSec() < jointTrajectory.points[i].time_from_start.toSec() )
    {
      continue;
    }
    int32_t speed = dxl_wb_->convertVelocity2Value(dxl_id_[index], jointTrajectory.points[i].velocities[0]);
    int32_t position = dxl_wb_->convertRadian2Value(dxl_id_[index], jointTrajectory.points[i].positions[0]);
    dxl_wb_->itemWrite(dxl_id_[index], "Profile_Velocity", speed);
    dxl_wb_->goalPosition(dxl_id_[index], position);
    ROS_INFO("Writing position to Dynamixel motor #%d: [%d, %d] at time %f.", dxl_id_[index], position, speed, (ros::Time::now() - trajectoryStartTime).toSec());
  }