From 3ab28b11631fc927502d75a6f82c520ca61c5b8c Mon Sep 17 00:00:00 2001
From: Simon Gabl <simon.gabl@franka.de>
Date: Tue, 23 Jan 2018 13:55:02 +0100
Subject: [PATCH] Update Cartesian impedance example controller.
---
.../launch/scripts/interactive_marker.py | 9 +++++----
.../src/cartesian_impedance_example_controller.cpp | 4 ++--
2 files changed, 7 insertions(+), 6 deletions(-)
diff --git a/franka_example_controllers/launch/scripts/interactive_marker.py b/franka_example_controllers/launch/scripts/interactive_marker.py
index d691044..55d163f 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 d3c4cdc..5e96ecb 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);
--
GitLab