From 221c2196b2e8a6572f890ec9479142d56d8b48c0 Mon Sep 17 00:00:00 2001
From: Jose Medina <jose.medina@franka.de>
Date: Mon, 29 Jan 2018 17:12:27 +0100
Subject: [PATCH] Remove unused gravity variable and add orientation check
---
.../src/cartesian_impedance_example_controller.cpp | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
index fd5c0ca..286cad5 100644
--- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
+++ b/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
--
GitLab