Update Cartesian impedance example controller.

franka_example_controllers/launch/scripts/interactive_marker.py

@@ -27,15 +27,16 @@ def publisherCallback(msg, arm_id):
 def franka_state_callback(msg):
     initial_quaternion = \
         tf.transformations.quaternion_from_matrix(
-            np.transpose(np.reshape(msg.O_T_EE,
+            np.transpose(np.reshape(msg.O_T_EE_d,
                                     (4, 4))))
+    initial_quaternion = initial_quaternion / np.linalg.norm(initial_quaternion)
     marker_pose.pose.orientation.x = initial_quaternion[0]
     marker_pose.pose.orientation.y = initial_quaternion[1]
     marker_pose.pose.orientation.z = initial_quaternion[2]
     marker_pose.pose.orientation.w = initial_quaternion[3]
-    marker_pose.pose.position.x = msg.O_T_EE[12]
-    marker_pose.pose.position.y = msg.O_T_EE[13]
-    marker_pose.pose.position.z = msg.O_T_EE[14]
+    marker_pose.pose.position.x = msg.O_T_EE_d[12]
+    marker_pose.pose.position.y = msg.O_T_EE_d[13]
+    marker_pose.pose.position.z = msg.O_T_EE_d[14]
     global initial_pose_found
     initial_pose_found = True
 

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

@@ -136,9 +136,9 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
   // get state variables
   franka::RobotState robot_state = state_handle_->getRobotState();
   std::array<double, 7> gravity_array =
-      model_handle_->getGravity(robot_state.m_load, robot_state.F_x_Cload);
+      model_handle_->getGravity(robot_state.m_total, robot_state.F_x_Ctotal);
   std::array<double, 7> coriolis_array =
-      model_handle_->getCoriolis(robot_state.I_load, robot_state.m_load, robot_state.F_x_Cload);
+      model_handle_->getCoriolis(robot_state.I_total, robot_state.m_total, robot_state.F_x_Ctotal);
   std::array<double, 42> jacobian_array =
       model_handle_->getZeroJacobian(franka::Frame::kEndEffector, robot_state);