add support for speedj

ur_robot_driver/config/ur10_controllers.yaml

@@ -60,3 +60,58 @@ pos_traj_controller:
    stop_trajectory_duration: 0.5
    state_publish_rate: *loop_hz
    action_monitor_rate: 10
+
+vel_based_pos_traj_controller:
+   type: velocity_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
+      elbow_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate:  125
+   action_monitor_rate: 10
+   gains:
+      #!!These values have not been optimized!!
+      shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+
+   # Use a feedforward term to reduce the size of PID gains
+   velocity_ff:
+      shoulder_pan_joint: 1.0
+      shoulder_lift_joint: 1.0
+      elbow_joint: 1.0
+      wrist_1_joint: 1.0
+      wrist_2_joint: 1.0
+      wrist_3_joint: 1.0
+
+   # state_publish_rate:  50 # Defaults to 50
+   # action_monitor_rate: 20 # Defaults to 20
+   #stop_trajectory_duration: 0 # Defaults to 0.0
+
+# Pass an array of joint velocities directly to the joints
+joint_group_vel_controller:
+   type: velocity_controllers/JointGroupVelocityController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -203,11 +203,11 @@ protected:
   ur_controllers::ScaledPositionJointInterface spj_interface_;
   hardware_interface::PositionJointInterface pj_interface_;
   ur_controllers::SpeedScalingInterface speedsc_interface_;
-  // hardware_interface::VelocityJointInterface vj_interface_;
+  hardware_interface::VelocityJointInterface vj_interface_;
   hardware_interface::ForceTorqueSensorInterface fts_interface_;
 
   vector6d_t joint_position_command_;
-  // std::vector<double> joint_velocity_command_;
+  vector6d_t joint_velocity_command_;
   vector6d_t joint_positions_;
   vector6d_t joint_velocities_;
   vector6d_t joint_efforts_;
@@ -247,6 +247,7 @@ protected:
 
   uint32_t runtime_state_;
   bool position_controller_running_;
+  bool velocity_controller_running_;
 
   PausingState pausing_state_;
   double pausing_ramp_up_increment_;

ur_robot_driver/launch/ur10_speed_bringup.launch

+<?xml version="1.0"?>
+<launch>
+
+  <!-- GDB functionality -->
+  <arg name="debug" default="false" />
+
+  <arg name="controller_config_file" default="$(find ur_robot_driver)/config/ur10_controllers.yaml"/>
+  <arg name="robot_description_file" default="$(find ur_description)/launch/ur10_upload.launch"/>
+  <arg name="kinematics_config" default="$(find ur_description)/config/ur10_default.yaml"/>
+  <arg name="robot_ip"/>
+  <arg name="limited" default="false"/>
+  <arg name="tf_prefix" default="" />
+  <arg name="controllers" default="joint_state_controller joint_group_vel_controller force_torque_sensor_controller"/>
+  <arg name="stopped_controllers" default="vel_based_pos_traj_controller"/>
+
+  <include file="$(find ur_robot_driver)/launch/ur_common.launch">
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="use_tool_communication" value="false"/>
+    <arg name="controller_config_file" value="$(arg controller_config_file)"/>
+    <arg name="robot_description_file" value="$(arg robot_description_file)"/>
+    <arg name="robot_ip" value="$(arg robot_ip)"/>
+    <arg name="kinematics_config" value="$(arg kinematics_config)"/>
+    <arg name="limited" value="$(arg limited)"/>
+    <arg name="tf_prefix" value="$(arg tf_prefix)"/>
+    <arg name="controllers" value="$(arg controllers)"/>
+    <arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
+  </include>
+
+</launch>

ur_robot_driver/launch/ur_control.launch

@@ -13,9 +13,9 @@
   <arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
   <arg name="kinematics_config" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description. Pass the same config file that is passed to the robot_description."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
-  <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
-  <arg name="stopped_controllers" default="pos_traj_controller" doc="Controllers that are initally loaded, but not started."/>
-  <arg name="urscript_file" default="$(find ur_robot_driver)/resources/servoj.urscript" doc="Path to URScript that will be sent to the robot and that forms the main control program."/>
+  <arg name="controllers" default="joint_state_controller vel_based_pos_traj_controller force_torque_sensor_controller"/>
+  <arg name="stopped_controllers" default="joint_group_vel_controller"/>
+  <arg name="urscript_file" default="$(find ur_robot_driver)/resources/speedj.urscript"/>
   <arg name="rtde_output_recipe_file" default="$(find ur_robot_driver)/resources/rtde_output_recipe.txt" doc="Recipe file used for the RTDE-outputs. Only change this if you know what you're doing."/>
   <arg name="rtde_input_recipe_file" default="$(find ur_robot_driver)/resources/rtde_input_recipe.txt" doc="Recipe file used for the RTDE-inputs. Only change this if you know what you're doing."/>
   <arg name="tool_voltage" default="0" doc="Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true."/>

ur_robot_driver/resources/speedj.urscript

+{{BEGIN_REPLACE}}
+
+global steptime = get_steptime()
+global speedj_time = steptime - 0.1*steptime
+textmsg("steptime=", steptime)
+MULT_jointstate = {{JOINT_STATE_REPLACE}}
+
+global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+cmd_speedj_active = True
+keepalive = -2
+
+def set_speed(qd):
+  enter_critical
+  cmd_servo_qd = qd
+  cmd_speedj_active = True
+  exit_critical
+end
+
+thread speedThread():
+  while True:
+    enter_critical
+    qd = cmd_servo_qd
+    exit_critical
+    #textmsg("loop")
+    if cmd_speedj_active:
+      # Having this shortly under steptime seems to help
+      speedj(qd, 40.0, speedj_time)
+    else:
+      stopj(5.0)
+    end
+    sync()
+  end
+  stopj(5.0)
+end
+
+socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
+
+thread_speed = run speedThread()
+
+params_mult = socket_read_binary_integer(1+6, "reverse_socket")
+keepalive = params_mult[1]
+while keepalive > 0:
+  params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02)
+  if params_mult[0] > 0:
+    keepalive = params_mult[1]
+    if keepalive > 1:
+      qd = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate]
+      set_speed(qd)
+      #speedj(qd, 40.0, 0.007)
+    else:
+      cmd_speedj_active = False
+      keepalive = 1
+    end
+  else:
+    textmsg("Missing package from remote pc")
+    keepalive = keepalive - 1
+  end
+end
+sleep(.1)
+socket_close()
+kill thread_speed
+

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -35,6 +35,7 @@ namespace ur_driver
 {
 HardwareInterface::HardwareInterface()
   : joint_position_command_({ 0, 0, 0, 0, 0, 0 })
+  , joint_velocity_command_({ 0, 0, 0, 0, 0, 0 })
   , joint_positions_{ { 0, 0, 0, 0, 0, 0 } }
   , joint_velocities_{ { 0, 0, 0, 0, 0, 0 } }
   , joint_efforts_{ { 0, 0, 0, 0, 0, 0 } }
@@ -44,6 +45,7 @@ HardwareInterface::HardwareInterface()
   , safety_mode_(ur_dashboard_msgs::SafetyMode::NORMAL)
   , runtime_state_(static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::STOPPED))
   , position_controller_running_(false)
+  , velocity_controller_running_(false)
   , pausing_state_(PausingState::RUNNING)
   , pausing_ramp_up_increment_(0.01)
   , controllers_initialized_(false)
@@ -281,6 +283,8 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
     // Create joint position control interface
     pj_interface_.registerHandle(
         hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
+    vj_interface_.registerHandle(
+        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]));
     spj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
         js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_));
   }
@@ -295,6 +299,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   registerInterface(&js_interface_);
   registerInterface(&spj_interface_);
   registerInterface(&pj_interface_);
+  registerInterface(&vj_interface_);
   registerInterface(&speedsc_interface_);
   registerInterface(&fts_interface_);
 
@@ -473,6 +478,10 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
     {
       ur_driver_->writeJointCommand(joint_position_command_);
     }
+    else if (velocity_controller_running_)
+    {
+      ur_driver_->writeJointCommand(joint_velocity_command_);
+    }
     else if (robot_program_running_)
     {
       ur_driver_->writeKeepalive();
@@ -511,6 +520,7 @@ void HardwareInterface::doSwitch(const std::list<hardware_interface::ControllerI
                                  const std::list<hardware_interface::ControllerInfo>& stop_list)
 {
   position_controller_running_ = false;
+  velocity_controller_running_ = false;
   for (auto& controller_it : start_list)
   {
     for (auto& resource_it : controller_it.claimed_resources)
@@ -523,6 +533,10 @@ void HardwareInterface::doSwitch(const std::list<hardware_interface::ControllerI
       {
         position_controller_running_ = true;
       }
+      if (resource_it.hardware_interface == "hardware_interface::VelocityJointInterface")
+      {
+        velocity_controller_running_ = true;
+      }
     }
   }
 }

ur_robot_driver/src/ur/ur_driver.cpp

@@ -87,13 +87,27 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
   std::string local_ip = rtde_client_->getIP();
 
   std::string prog = readScriptFile(script_file);
-  prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE));
+  while (prog.find(JOINT_STATE_REPLACE) != std::string::npos)
+  {
+    prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE));
+  }
+
   std::ostringstream out;
   out << "lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
-  prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
-  prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
-  prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip);
-  prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
+  while (prog.find(SERVO_J_REPLACE) != std::string::npos)
+  {
+    prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
+  }
+
+  while (prog.find(SERVER_IP_REPLACE) != std::string::npos)
+  {
+    prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip);
+  }
+
+  while (prog.find(SERVER_PORT_REPLACE) != std::string::npos)
+  {
+    prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
+  }
 
   robot_version_ = rtde_client_->getVersion();