在Ubuntu 16.04和ROS Kinetic上使用UR3 Robot(2)

// Resize vectors
 joint_position_.resize(num_joints_);
 joint_velocity_.resize(num_joints_);
 joint_effort_.resize(num_joints_);
 joint_position_command_.resize(num_joints_);
 joint_velocity_command_.resize(num_joints_);
 prev_joint_velocity_command_.resize(num_joints_);

// Initialize controller
 for (std::size_t i = 0; i < num_joints_; ++i) {
  ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
    "Loading joint name: " << joint_names_[i]);

// Create joint state interface
  joint_state_interface_.registerHandle(
    hardware_interface::JointStateHandle(joint_names_[i],
      &joint_position_[i], &joint_velocity_[i],
      &joint_effort_[i]));

// Create position joint interface
  position_joint_interface_.registerHandle(
    hardware_interface::JointHandle(
      joint_state_interface_.getHandle(joint_names_[i]),
      &joint_position_command_[i]));

// Create velocity joint interface
  velocity_joint_interface_.registerHandle(
    hardware_interface::JointHandle(
      joint_state_interface_.getHandle(joint_names_[i]),
      &joint_velocity_command_[i]));
  prev_joint_velocity_command_[i] = 0.;
 }

// Create force torque interface
 force_torque_interface_.registerHandle(
   hardware_interface::ForceTorqueSensorHandle("wrench", "",
     robot_force_, robot_torque_));

registerInterface(&joint_state_interface_); // From RobotHW base class.
 registerInterface(&position_joint_interface_); // From RobotHW base class.
 registerInterface(&velocity_joint_interface_); // From RobotHW base class.
 registerInterface(&force_torque_interface_); // From RobotHW base class.
 velocity_interface_running_ = false;
 position_interface_running_ = false;
}

void UrHardwareInterface::read() {
 std::vector<double> pos, vel, current, tcp;
 pos = robot_->rt_interface_->robot_state_->getQActual();
 vel = robot_->rt_interface_->robot_state_->getQdActual();
 current = robot_->rt_interface_->robot_state_->getIActual();
 tcp = robot_->rt_interface_->robot_state_->getTcpForce();
 for (std::size_t i = 0; i < num_joints_; ++i) {
  joint_position_[i] = pos[i];
  joint_velocity_[i] = vel[i];
  joint_effort_[i] = current[i];
 }
 for (std::size_t i = 0; i < 3; ++i) {
  robot_force_[i] = tcp[i];
  robot_torque_[i] = tcp[i + 3];
 }

}

void UrHardwareInterface::setMaxVelChange(double inp) {
 max_vel_change_ = inp;
}

void UrHardwareInterface::write() {
 if (velocity_interface_running_) {
  std::vector<double> cmd;
  //do some rate limiting
  cmd.resize(joint_velocity_command_.size());
  for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) {
   cmd[i] = joint_velocity_command_[i];
   if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) {
    cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_;
   } else if (cmd[i]
     < prev_joint_velocity_command_[i] - max_vel_change_) {
    cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_;
   }
   prev_joint_velocity_command_[i] = cmd[i];
  }
  robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5],  max_vel_change_*125);
 } else if (position_interface_running_) {
  robot_->servoj(joint_position_command_);
 }
}

bool UrHardwareInterface::canSwitch(
  const std::list<hardware_interface::ControllerInfo> &start_list,
  const std::list<hardware_interface::ControllerInfo> &stop_list) const {
 for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
   start_list.begin(); controller_it != start_list.end();
   ++controller_it) {
  if (controller_it->type
    == "hardware_interface::VelocityJointInterface") {
   if (velocity_interface_running_) {
    ROS_ERROR(
      "%s: An interface of that type (%s) is already running",
      controller_it->name.c_str(),
      controller_it->type.c_str());
    return false;
   }
   if (position_interface_running_) {
    bool error = true;
    for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it =
      stop_list.begin();
      stop_controller_it != stop_list.end();
      ++stop_controller_it) {
     if (stop_controller_it->type
       == "hardware_interface::PositionJointInterface") {
      error = false;
      break;
     }
    }
    if (error) {
     ROS_ERROR(
       "%s (type %s) can not be run simultaneously with a PositionJointInterface",
       controller_it->name.c_str(),
       controller_it->type.c_str());
     return false;
    }
   }
  } else if (controller_it->type
    == "hardware_interface::PositionJointInterface") {
   if (position_interface_running_) {
    ROS_ERROR(
      "%s: An interface of that type (%s) is already running",
      controller_it->name.c_str(),
      controller_it->type.c_str());
    return false;
   }
   if (velocity_interface_running_) {
    bool error = true;
    for (std::list<hardware_interface::ControllerInfo>::const_iterator stop_controller_it =
      stop_list.begin();
      stop_controller_it != stop_list.end();
      ++stop_controller_it) {
     if (stop_controller_it->type
       == "hardware_interface::VelocityJointInterface") {
      error = false;
      break;
     }
    }
    if (error) {
     ROS_ERROR(
       "%s (type %s) can not be run simultaneously with a VelocityJointInterface",
       controller_it->name.c_str(),
       controller_it->type.c_str());
     return false;
    }
   }
  }
 }

// we can always stop a controller
 return true;
}

内容版权声明:除非注明,否则皆为本站原创文章。

转载注明出处:https://www.heiqu.com/14176.html