franka调试记录

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <franka_example_controllers/joint_velocity_example_controller.h>

#include <cmath>

#include <controller_interface/controller_base.h>
#include <hardware_interface/hardware_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>

#include <iostream>  
#include <fstream>  
#include <sstream>  
#include <vector>  

namespace franka_example_controllers {

bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware,
                                          ros::NodeHandle& node_handle) {
  velocity_joint_interface_ = robot_hardware->get<hardware_interface::VelocityJointInterface>();
  if (velocity_joint_interface_ == nullptr) {
    ROS_ERROR(
        "JointVelocityExampleController: Error getting velocity joint interface from hardware!");
    return false;
  }

  std::string arm_id;
  if (!node_handle.getParam("arm_id", arm_id)) {
    ROS_ERROR("JointVelocityExampleController: Could not get parameter arm_id");
    return false;
  }

  std::vector<std::string> joint_names;
  if (!node_handle.getParam("joint_names", joint_names)) {
    ROS_ERROR("JointVelocityExampleController: Could not parse joint names");
  }
  if (joint_names.size() != 7) {
    ROS_ERROR_STREAM("JointVelocityExampleController: Wrong number of joint names, got "
                     << joint_names.size() << " instead of 7 names!");
    return false;
  }
  velocity_joint_handles_.resize(7);
  for (size_t i = 0; i < 7; ++i) {
    try {
      velocity_joint_handles_[i] = velocity_joint_interface_->getHandle(joint_names[i]);
    } catch (const hardware_interface::HardwareInterfaceException& ex) {
      ROS_ERROR_STREAM(
          "JointVelocityExampleController: Exception getting joint handles: " << ex.what());
      return false;
    }
  }

  auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
  if (state_interface == nullptr) {
    ROS_ERROR("JointVelocityExampleController: Could not get state interface from hardware");
    return false;
  }

  try {
    auto state_handle = state_interface->getHandle(arm_id + "_robot");

    std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
    for (size_t i = 0; i < q_start.size(); i++) {
      if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
        ROS_ERROR_STREAM(
            "JointVelocityExampleController: Robot is not in the expected starting position for "
            "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
            "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
        return false;
      }
    }
  } catch (const hardware_interface::HardwareInterfaceException& e) {
    ROS_ERROR_STREAM(
        "JointVelocityExampleController: Exception getting state handle: " << e.what());
    return false;
  }


  return true;
}

void JointVelocityExampleController::starting(const ros::Time& /* time */) {
  elapsed_time_ = ros::Duration(0.0);
}


void JointVelocityExampleController::update(const ros::Time& /* time */,
                                            const ros::Duration& period) {

  elapsed_time_ += period;  

  if (!csvFile.is_open()) {  
    csvFile.open("/home/robuster/Tan/fr3_ws/src/franka_ros/franka_example_controllers/scripts/cmdsim2real.csv");  
    if (!csvFile.is_open()) {  
      ROS_ERROR("Unable to open data.csv file");  
      return;  
    }  
  }  
  
  // 读取一行数据  
  std::string line;  
  std::getline(csvFile, line);  
  if (csvFile.eof()) {  
    // 文件结束，关闭文件  
    csvFile.close();  
    ROS_INFO("Reached end of file");  
    line = "0,0,0,0,0,0,0,0,0";
    return;  
  }  
  
  // 使用istringstream分割行数据  
  std::istringstream iss(line);  
  std::string cell,tmp;  
  joint_velocities.clear(); // 清空vector以准备存储新行数据  
    
  // 忽略前两个数据（如果需要的话）  
  std::getline(iss, cell, ',');
  std::getline(iss, cell, ',');
  
  // // 读取后七个数据  
  while (std::getline(iss, tmp, ',')){
    std::cout<<tmp;
    double value = std::stod(tmp);
    joint_velocities.push_back(value);} 
  
  for (int i = 0; i < 7; ++i) {  
    velocity_joint_handles_[i].setCommand(joint_velocities[i]);  
  }


  // elapsed_time_ += period;
  // ros::Duration time_max(8.0);
  // double omega_max = 0.1;
  // double cycle = std::floor(
  //     std::pow(-1.0, (elapsed_time_.toSec() - std::fmod(elapsed_time_.toSec(), time_max.toSec())) /
  //                        time_max.toSec()));
  // double omega = cycle * omega_max / 2.0 *
  //                (1.0 - std::cos(2.0 * M_PI / time_max.toSec() * elapsed_time_.toSec()));

  // for (auto joint_handle : velocity_joint_handles_) {
  //   joint_handle.setCommand(omega);
  // }
}

void JointVelocityExampleController::stopping(const ros::Time& /*time*/) {
  // WARNING: DO NOT SEND ZERO VELOCITIES HERE AS IN CASE OF ABORTING DURING MOTION
  // A JUMP TO ZERO WILL BE COMMANDED PUTTING HIGH LOADS ON THE ROBOT. LET THE DEFAULT
  // BUILT-IN STOPPING BEHAVIOR SLOW DOWN THE ROBOT.
}

}  // namespace franka_example_controllers

PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointVelocityExampleController,
                       controller_interface::ControllerBase)

主要问题：

1.不断刷新：Reached end of file

2.机械臂读取完成csv后不停止

3.结束速度快

csv：

0.634678 0.480001 -0.692958 0.150932 -0.480184 0.086951 -0.123682 0.053077 0.803284
0.134477 0.487173 -0.183985 0.147885 -0.075933 0.084993 0.034701 0.051675 0.472653
0.134145 0.485879 -0.182234 0.147492 -0.074019 0.085129 0.035141 0.051037 0.473791
0.133810 0.484651 -0.180511 0.147109 -0.072139 0.085264 0.035568 0.050412 0.474925
0.133471 0.483487 -0.178816 0.146734 -0.070294 0.085397 0.035982 0.049801 0.476057
0.133130 0.482387 -0.177147 0.146368 -0.068482 0.085528 0.036384 0.049204 0.477185
0.132784 0.481349 -0.175505 0.146011 -0.066702 0.085659 0.036773 0.048619 0.478311
0.132434 0.480370 -0.173887 0.145663 -0.064954 0.085789 0.037151 0.048048 0.479433
0.132080 0.479449 -0.172293 0.145324 -0.063237 0.085918 0.037517 0.047489 0.480553
0.131722 0.478586 -0.170723 0.144993 -0.061549 0.086047 0.037871 0.046942 0.481670
0.131358 0.477778 -0.169176 0.144671 -0.059890 0.086175 0.038215 0.046408 0.482785
0.130989 0.477025 -0.167650 0.144359 -0.058260 0.086304 0.038548 0.045885 0.483897
0.130614 0.476324 -0.166146 0.144054 -0.056657 0.086433 0.038871 0.045373 0.485008
0.130233 0.475675 -0.164663 0.143759 -0.055081 0.086562 0.039183 0.044873 0.486116
0.129846 0.475076 -0.163200 0.143473 -0.053531 0.086692 0.039486 0.044384 0.487222
0.129453 0.474527 -0.161756 0.143195 -0.052006 0.086822 0.039779 0.043905 0.488326
0.129053 0.474026 -0.160331 0.142926 -0.050506 0.086953 0.040062 0.043438 0.489429
0.128646 0.473571 -0.158924 0.142665 -0.049030 0.087085 0.040336 0.042980 0.490530

......