Merge pull request #63 in SWDEV/franka_ros from tau-j-d-without-gravity to develop

* commit 'aef60ee7':
  Jenkinsfile: Fix branch fallback
  Remove unused gravity variable and add orientation check
  Fix typo.
  Removed gravity from tau_J_d and adjusted examples.
  Added three new errors to franka_msgs::Errors.
  Added tau_J_d, theta and dtheta to the FrankaState message.

CHANGELOG.md

@@ -2,10 +2,13 @@
 
 ## 0.2.0 - UNRELEASED
 
+Requires `libfranka` >= 0.2.0
+
   * Added missing run-time dependencies to `franka_description` and `franka_control`
-  * Added `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal` and `I_total`
-    to the robot state
-  * Updated and improved examples
+  * Added `tau_J_d`, `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal`, `I_total`,
+    `theta` and `dtheta` to `franka_msgs/FrankaState`
+  * Added new errors to `franka_msgs/Errors`
+  * Updated and improved examples in `franka_example_controllers`
   * Fixed includes for Eigen3 in `franka_example_controllers`
   * Fixed gripper state publishing in `franka_gripper_node`
 

Jenkinsfile

@@ -17,11 +17,11 @@ node('docker') {
               projectName: "SWDEV/libfranka/${java.net.URLEncoder.encode(env.BRANCH_NAME, "UTF-8")}",
               selector: [$class: 'StatusBuildSelector', stable: false]])
       } catch (e) {
-        // Fall back to master branch.
+        // Fall back to develop branch.
         step([$class: 'CopyArtifact',
               filter: 'libfranka-*-amd64.tar.gz',
               fingerprintArtifacts: true,
-              projectName: "SWDEV/libfranka/master",
+              projectName: "SWDEV/libfranka/develop",
               selector: [$class: 'StatusBuildSelector', stable: false]])
       }
       sh '''

franka_control/src/franka_state_controller.cpp

@@ -120,6 +120,13 @@ franka_msgs::Errors errorsToMessage(const franka::Errors& error) {
           error.communication_constraints_violation);
   message.power_limit_violation =
       static_cast<decltype(message.power_limit_violation)>(error.power_limit_violation);
+  message.joint_p2p_insufficient_torque_for_planning =
+      static_cast<decltype(message.joint_p2p_insufficient_torque_for_planning)>(
+          error.joint_p2p_insufficient_torque_for_planning);
+  message.tau_j_range_violation =
+      static_cast<decltype(message.tau_j_range_violation)>(error.tau_j_range_violation);
+  message.instability_detected =
+      static_cast<decltype(message.instability_detected)>(error.instability_detected);
   return message;
 }
 
@@ -255,6 +262,12 @@ void FrankaStateController::publishFrankaStates(const ros::Time& time) {
                   "Robot state joint members do not have same size");
     static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J),
                   "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J_d),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.theta),
+                  "Robot state joint members do not have same size");
+    static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtheta),
+                  "Robot state joint members do not have same size");
     static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision),
                   "Robot state joint members do not have same size");
     static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact),
@@ -268,6 +281,9 @@ void FrankaStateController::publishFrankaStates(const ros::Time& time) {
       publisher_franka_states_.msg_.dq_d[i] = robot_state_.dq_d[i];
       publisher_franka_states_.msg_.tau_J[i] = robot_state_.tau_J[i];
       publisher_franka_states_.msg_.dtau_J[i] = robot_state_.dtau_J[i];
+      publisher_franka_states_.msg_.tau_J_d[i] = robot_state_.tau_J_d[i];
+      publisher_franka_states_.msg_.theta[i] = robot_state_.theta[i];
+      publisher_franka_states_.msg_.dtheta[i] = robot_state_.dtheta[i];
       publisher_franka_states_.msg_.joint_collision[i] = robot_state_.joint_collision[i];
       publisher_franka_states_.msg_.joint_contact[i] = robot_state_.joint_contact[i];
       publisher_franka_states_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i];

franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h

@@ -34,8 +34,7 @@ class CartesianImpedanceExampleController : public controller_interface::MultiIn
   // Saturation
   Eigen::Matrix<double, 7, 1> saturateTorqueRate(
       const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
-      const Eigen::Matrix<double, 7, 1>& tau_J_d,  // NOLINT (readability-identifier-naming)
-      const Eigen::Matrix<double, 7, 1>& gravity);
+      const Eigen::Matrix<double, 7, 1>& tau_J_d);  // NOLINT (readability-identifier-naming)
 
   std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
   std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;

franka_example_controllers/include/franka_example_controllers/force_example_controller.h

@@ -33,8 +33,7 @@ class ForceExampleController : public controller_interface::MultiInterfaceContro
   // Saturation
   Eigen::Matrix<double, 7, 1> saturateTorqueRate(
       const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
-      const Eigen::Matrix<double, 7, 1>& tau_J_d,  // NOLINT (readability-identifier-naming)
-      const Eigen::Matrix<double, 7, 1>& gravity);
+      const Eigen::Matrix<double, 7, 1>& tau_J_d);  // NOLINT (readability-identifier-naming)
 
   std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
   std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;

franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h

@@ -33,8 +33,7 @@ class JointImpedanceExampleController : public controller_interface::MultiInterf
   // Saturation
   std::array<double, 7> saturateTorqueRate(
       const std::array<double, 7>& tau_d_calculated,
-      const std::array<double, 7>& tau_J_d,  // NOLINT (readability-identifier-naming)
-      const std::array<double, 7>& gravity);
+      const std::array<double, 7>& tau_J_d);  // NOLINT (readability-identifier-naming)
 
   std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
   std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

@@ -134,8 +134,6 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
                                                  const ros::Duration& /*period*/) {
   // get state variables
   franka::RobotState robot_state = state_handle_->getRobotState();
-  std::array<double, 7> gravity_array =
-      model_handle_->getGravity(robot_state.m_total, robot_state.F_x_Ctotal);
   std::array<double, 7> coriolis_array =
       model_handle_->getCoriolis(robot_state.I_total, robot_state.m_total, robot_state.F_x_Ctotal);
   std::array<double, 42> jacobian_array =
@@ -143,7 +141,6 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
 
   // convert to Eigen
   Eigen::Map<Eigen::Matrix<double, 7, 1> > coriolis(coriolis_array.data());
-  Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data());
   Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
   Eigen::Map<Eigen::Matrix<double, 7, 1> > q(robot_state.q.data());
   Eigen::Map<Eigen::Matrix<double, 7, 1> > dq(robot_state.dq.data());
@@ -159,6 +156,9 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
   error.head(3) << position - position_d_;
 
   // orientation error
+  if (orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) {
+    orientation.coeffs() << -orientation.coeffs();
+  }
   // "difference" quaternion
   Eigen::Quaterniond error_quaternion(orientation * orientation_d_.inverse());
   // convert to axis angle
@@ -186,7 +186,7 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
   // Desired torque
   tau_d << tau_task + tau_nullspace + coriolis;
   // Saturate torque rate to avoid discontinuities
-  tau_d << saturateTorqueRate(tau_d, tau_J_d, gravity);
+  tau_d << saturateTorqueRate(tau_d, tau_J_d);
   for (size_t i = 0; i < 7; ++i) {
     joint_handles_[i].setCommand(tau_d(i));
   }
@@ -211,14 +211,12 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
 
 Eigen::Matrix<double, 7, 1> CartesianImpedanceExampleController::saturateTorqueRate(
     const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
-    const Eigen::Matrix<double, 7, 1>& tau_J_d,  // NOLINT (readability-identifier-naming)
-    const Eigen::Matrix<double, 7, 1>& gravity) {
+    const Eigen::Matrix<double, 7, 1>& tau_J_d) {  // NOLINT (readability-identifier-naming)
   Eigen::Matrix<double, 7, 1> tau_d_saturated{};
   for (size_t i = 0; i < 7; i++) {
-    // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more.
-    double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]);
+    double difference = tau_d_calculated[i] - tau_J_d[i];
     tau_d_saturated[i] =
-        (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_);
+        tau_J_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_);
   }
   return tau_d_saturated;
 }

franka_example_controllers/src/force_example_controller.cpp

@@ -115,9 +115,9 @@ void ForceExampleController::update(const ros::Time& /*time*/, const ros::Durati
   tau_ext = tau_measured - gravity - tau_ext_initial_;
   tau_d << jacobian.transpose() * desired_force_torque;
   tau_error_ = tau_error_ + period.toSec() * (tau_d - tau_ext);
-  // FF + PI control (PI gains are intially all 0)
+  // FF + PI control (PI gains are initially all 0)
   tau_cmd = tau_d + k_p_ * (tau_d - tau_ext) + k_i_ * tau_error_;
-  tau_cmd << saturateTorqueRate(tau_cmd, tau_J_d, gravity);
+  tau_cmd << saturateTorqueRate(tau_cmd, tau_J_d);
 
   for (size_t i = 0; i < 7; ++i) {
     joint_handles_[i].setCommand(tau_cmd(i));
@@ -139,14 +139,11 @@ void ForceExampleController::desiredMassParamCallback(
 
 Eigen::Matrix<double, 7, 1> ForceExampleController::saturateTorqueRate(
     const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
-    const Eigen::Matrix<double, 7, 1>& tau_J_d,  // NOLINT (readability-identifier-naming)
-    const Eigen::Matrix<double, 7, 1>& gravity) {
+    const Eigen::Matrix<double, 7, 1>& tau_J_d) {  // NOLINT (readability-identifier-naming)
   Eigen::Matrix<double, 7, 1> tau_d_saturated{};
   for (size_t i = 0; i < 7; i++) {
-    // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more.
-    double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]);
-    tau_d_saturated[i] =
-        (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);
+    double difference = tau_d_calculated[i] - tau_J_d[i];
+    tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);
   }
   return tau_d_saturated;
 }

franka_example_controllers/src/joint_impedance_example_controller.cpp

@@ -173,8 +173,7 @@ void JointImpedanceExampleController::update(const ros::Time& /*time*/,
 
   // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is
   // 1000 * (1 / sampling_time).
-  std::array<double, 7> tau_d_saturated =
-      saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d, gravity);
+  std::array<double, 7> tau_d_saturated = saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d);
 
   for (size_t i = 0; i < 7; ++i) {
     joint_handles_[i].setCommand(tau_d_saturated[i]);
@@ -204,14 +203,11 @@ void JointImpedanceExampleController::update(const ros::Time& /*time*/,
 
 std::array<double, 7> JointImpedanceExampleController::saturateTorqueRate(
     const std::array<double, 7>& tau_d_calculated,
-    const std::array<double, 7>& tau_J_d,  // NOLINT (readability-identifier-naming)
-    const std::array<double, 7>& gravity) {
+    const std::array<double, 7>& tau_J_d) {  // NOLINT (readability-identifier-naming)
   std::array<double, 7> tau_d_saturated{};
   for (size_t i = 0; i < 7; i++) {
-    // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more.
-    double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]);
-    tau_d_saturated[i] =
-        (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);
+    double difference = tau_d_calculated[i] - tau_J_d[i];
+    tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax);
   }
   return tau_d_saturated;
 }

franka_msgs/msg/Errors.msg

@@ -31,3 +31,6 @@ bool controller_torque_discontinuity
 bool start_elbow_sign_inconsistent
 bool communication_constraints_violation
 bool power_limit_violation
+bool joint_p2p_insufficient_torque_for_planning
+bool tau_j_range_violation
+bool instability_detected

franka_msgs/msg/FrankaState.msg

@@ -5,8 +5,11 @@ float64[7] q
 float64[7] q_d
 float64[7] dq
 float64[7] dq_d
+float64[7] theta
+float64[7] dtheta
 float64[7] tau_J
 float64[7] dtau_J
+float64[7] tau_J_d
 float64[6] K_F_ext_hat_K
 float64[2] elbow
 float64[2] elbow_d