diff --git a/franka_example_controllers/launch/scripts/interactive_marker.py b/franka_example_controllers/launch/scripts/interactive_marker.py index d691044687fe32c0de31f5818d6521a97a3ffc80..55d163f821ab5419c44cbd47ca9602d286f0cc25 100755 --- a/franka_example_controllers/launch/scripts/interactive_marker.py +++ b/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 diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index d3c4cdc258eae5b500b85fa3bfa0fd171ca42fd1..5e96ecbe35d0933076edbdcc262df3d4b82fb364 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/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);