Support for Overriding With Continuous Plans

franka.sh

@@ -49,9 +49,9 @@ robot_name="FRANKA_${hostname_first_letter^}"
 status_channel="${robot_name}_STATUS"
 echo "Using hostname: $(hostname) to choose Franka name: ${robot_name}"
 
-# check if there is already a driver running. at the start of this script 
-# we try to kill any existing instances, but if there are multiple containers 
-# running on the same machine, or have an instance running on another 
+# check if there is already a driver running. at the start of this script
+# we try to kill any existing instances, but if there are multiple containers
+# running on the same machine, or have an instance running on another
 # networked machine, we don't want to duplicate
 if lcm-echo -n 1 -v 0 --timeout 0.1 ${status_channel}; then
     echo "A Franka driver instance is already running and publishing to ${status_channel}!"

franka.yaml

@@ -6,68 +6,71 @@ robot_parameters:
   robot_ip: '172.16.0.2'
   # limits taken from https://frankaemika.github.io/docs/control_parameters.html#constants
   robot_high_joint_limits: # rad
-      - 2.8973
-      - 1.7628
-      - 2.8973
-      - -0.0698
-      - 2.8973
-      - 3.7525
-      - 2.8973
+    - 2.8973
+    - 1.7628
+    - 2.8973
+    - -0.0698
+    - 2.8973
+    - 3.7525
+    - 2.8973
   robot_low_joint_limits: # rad
-      - -2.8973
-      - -1.7628
-      - -2.8973
-      - -3.0718
-      - -2.8973
-      - -0.0175
-      - -2.8973
+    - -2.8973
+    - -1.7628
+    - -2.8973
+    - -3.0718
+    - -2.8973
+    - -0.0175
+    - -2.8973
   robot_max_velocities: # rad/s
-      - 2.1750
-      - 2.1750
-      - 2.1750
-      - 2.1750
-      - 2.6100
-      - 2.6100
-      - 2.6100
+    - 2.1750
+    - 2.1750
+    - 2.1750
+    - 2.1750
+    - 2.6100
+    - 2.6100
+    - 2.6100
   robot_max_accelerations: # rad/s^2
-      - 15
-      - 7.5
-      - 10
-      - 12.5
-      - 15
-      - 20
-      - 20
+    - 15
+    - 7.5
+    - 10
+    - 12.5
+    - 15
+    - 20
+    - 20
   robot_max_jerks: # rad/s^3
-      - 7500
-      - 3750
-      - 5000
-      - 6250
-      - 7500
-      - 10000
-      - 10000
+    - 7500
+    - 3750
+    - 5000
+    - 6250
+    - 7500
+    - 10000
+    - 10000
   robot_max_torques: # N*m
-      - 87
-      - 87
-      - 87
-      - 87
-      - 12
-      - 12
-      - 12
+    - 87
+    - 87
+    - 87
+    - 87
+    - 12
+    - 12
+    - 12
   robot_max_torques_dot: # N*m/s
-      - 1000
-      - 1000
-      - 1000
-      - 1000
-      - 1000
-      - 1000
-      - 1000
+    - 1000
+    - 1000
+    - 1000
+    - 1000
+    - 1000
+    - 1000
+    - 1000
+  pos_continuity_err_tolerance: 0.0001
+  vel_continuity_err_tolerance: 0.001
+  acc_continuity_err_tolerance: 0.01
   stop_epsilon: 0.015 # used to calculate when robot counts as "stopped"
-  stop_margin: 0.6    # how long robot sleeps before ready to recieve continue
+  stop_margin: 0.6 # how long robot sleeps before ready to recieve continue
   simulated: false
   urdf: franka_robot_multibody_collision.urdf
   urdf_dir: ''
   world_frame: base_link
   gravity_vector:
-      - 0.0
-      - 0.0
-      - -1.0
+    - 0.0
+    - 0.0
+    - -1.0

franka_sim.yaml

@@ -6,68 +6,71 @@ robot_parameters:
   robot_ip: '192.168.1.1' # sim
   # limits taken from https://frankaemika.github.io/docs/control_parameters.html#constants
   robot_high_joint_limits: # rad
-      - 2.8973
-      - 1.7628
-      - 2.8973
-      - -0.0698
-      - 2.8973
-      - 3.7525
-      - 2.8973
+    - 2.8973
+    - 1.7628
+    - 2.8973
+    - -0.0698
+    - 2.8973
+    - 3.7525
+    - 2.8973
   robot_low_joint_limits: # rad
-      - -2.8973
-      - -1.7628
-      - -2.8973
-      - -3.0718
-      - -2.8973
-      - -0.0175
-      - -2.8973
+    - -2.8973
+    - -1.7628
+    - -2.8973
+    - -3.0718
+    - -2.8973
+    - -0.0175
+    - -2.8973
   robot_max_velocities: # rad/s
-      - 2.1750
-      - 2.1750
-      - 2.1750
-      - 2.1750
-      - 2.6100
-      - 2.6100
-      - 2.6100
+    - 2.1750
+    - 2.1750
+    - 2.1750
+    - 2.1750
+    - 2.6100
+    - 2.6100
+    - 2.6100
   robot_max_accelerations: # rad/s^2
-      - 15
-      - 7.5
-      - 10
-      - 12.5
-      - 15
-      - 20
-      - 20
+    - 15
+    - 7.5
+    - 10
+    - 12.5
+    - 15
+    - 20
+    - 20
   robot_max_jerks: # rad/s^3
-      - 7500
-      - 3750
-      - 5000
-      - 6250
-      - 7500
-      - 10000
-      - 10000
+    - 7500
+    - 3750
+    - 5000
+    - 6250
+    - 7500
+    - 10000
+    - 10000
   robot_max_torques: # N*m
-      - 87
-      - 87
-      - 87
-      - 87
-      - 12
-      - 12
-      - 12
+    - 87
+    - 87
+    - 87
+    - 87
+    - 12
+    - 12
+    - 12
   robot_max_torques_dot: # N*m/s
-      - 1000
-      - 1000
-      - 1000
-      - 1000
-      - 1000
-      - 1000
-      - 1000
+    - 1000
+    - 1000
+    - 1000
+    - 1000
+    - 1000
+    - 1000
+    - 1000
+  pos_continuity_err_tolerance: 0.001
+  vel_continuity_err_tolerance: 0.001
+  acc_continuity_err_tolerance: 0.01
   stop_epsilon: 0.015 # used to calculate when robot counts as "stopped"
-  stop_margin: 0.6    # how long robot sleeps before ready to recieve continue
+  stop_margin: 0.6 # how long robot sleeps before ready to recieve continue
   simulated: true
   urdf: franka_robot_multibody_collision.urdf
   urdf_dir: ''
   world_frame: base_link
   gravity_vector:
-      - 0.0
-      - 0.0
-      - -1.0
+    - 0.0
+    - 0.0
+    - -1.0

src/driver/communication_interface.cc

@@ -175,7 +175,7 @@ CommunicationInterface::PopNewPlan() {
                     new_plan_buffer_.utime));
   }
   std::scoped_lock<std::mutex> lock {robot_plan_mutex_};
-  // std::move nullifies the unique ptr robot_plan_.plan_
+  // std::move nullifies the unique ptr new_plan_buffer_.plan
   return {std::move(new_plan_buffer_.plan), new_plan_buffer_.utime,
           new_plan_buffer_.exec_opt, new_plan_buffer_.contact_expected};
 }
@@ -189,7 +189,7 @@ CommunicationInterface::PopNewCartesianPlan() {
                     new_plan_buffer_.utime));
   }
   std::scoped_lock<std::mutex> lock {robot_plan_mutex_};
-  // std::move nullifies the unique ptr robot_plan_.plan_
+  // std::move nullifies the unique ptr new_plan_buffer_.cartesian_plan
   return {std::move(new_plan_buffer_.cartesian_plan), new_plan_buffer_.utime,
           new_plan_buffer_.exec_opt};
 }
@@ -201,12 +201,14 @@ franka::RobotState CommunicationInterface::GetRobotState() {
 
 void CommunicationInterface::SetRobotData(
     const franka::RobotState& robot_state,
-    const Eigen::VectorXd& robot_plan_next_conf, int64_t current_plan_utime,
-    int64_t plan_start_utime, double plan_completion_frac) {
+    const Eigen::VectorXd& robot_plan_next_conf, double robot_time,
+    int64_t current_plan_utime, int64_t plan_start_utime,
+    double plan_completion_frac) {
   std::scoped_lock<std::mutex> lock {robot_data_mutex_};
   franka::RobotMode current_mode {robot_data_.robot_state.robot_mode};
   robot_data_.robot_state = robot_state;
   robot_data_.robot_plan_next_conf = robot_plan_next_conf;
+  robot_data_.robot_time = robot_time;
   robot_data_.current_plan_utime = current_plan_utime;
   robot_data_.plan_start_utime = plan_start_utime;
   robot_data_.plan_completion_frac = plan_completion_frac;

src/driver/communication_interface.h

@@ -78,9 +78,22 @@ namespace franka_driver {
 struct RobotData {
   std::atomic<bool> has_robot_data;
   franka::RobotState robot_state;
+
+  // time in seconds since start
+  // of current plan
+  double robot_time {};
+
+  // utime associated with current plan.
+  // defaults to -1 if no plan
   int64_t current_plan_utime {};
+
+  // utime at start of current plan
   int64_t plan_start_utime {};
-  double plan_completion_frac;
+
+  // plan completion fraction.
+  // should be in [0, 1]
+  double plan_completion_frac {};
+
   Eigen::VectorXd robot_plan_next_conf;
 };
 
@@ -168,7 +181,8 @@ class CommunicationInterface {
   // Set the robot state, blocking
   void SetRobotData(const franka::RobotState& robot_state,
                     const Eigen::VectorXd& robot_plan_next_conf,
-                    int64_t current_plan_utime, int64_t plan_start_utime = -1,
+                    double robot_time, int64_t current_plan_utime,
+                    int64_t plan_start_utime = -1,
                     double plan_completion_frac = 0.0);
 
   bool GetPauseStatus();