From 3ed307d9d9b1714b951f681e1ba85c1c56ceba1e Mon Sep 17 00:00:00 2001 From: Lefteris Kotsonis Date: Tue, 4 May 2021 15:32:36 +0200 Subject: [PATCH] Change sign of the gravity vector. --- force_torque_sensor_calib/src/ft_calib_node.cpp | 4 +++- gravity_compensation/src/gravity_compensation.cpp | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/force_torque_sensor_calib/src/ft_calib_node.cpp b/force_torque_sensor_calib/src/ft_calib_node.cpp index 5762ed1..02b4911 100644 --- a/force_torque_sensor_calib/src/ft_calib_node.cpp +++ b/force_torque_sensor_calib/src/ft_calib_node.cpp @@ -425,7 +425,9 @@ class FTCalibNode geometry_msgs::Vector3Stamped gravity; gravity.header.stamp = ros::Time(); gravity.header.frame_id = m_imu.header.frame_id; - gravity.vector = m_imu.linear_acceleration; + gravity.vector.x = -m_imu.linear_acceleration.x; // IMU will measure gravity in the opposite direction from F/T sensor, check https://github.com/kth-ros-pkg/force_torque_tools/pull/18 + gravity.vector.y = -m_imu.linear_acceleration.y; + gravity.vector.z = -m_imu.linear_acceleration.z; geometry_msgs::Vector3Stamped gravity_ft_frame; diff --git a/gravity_compensation/src/gravity_compensation.cpp b/gravity_compensation/src/gravity_compensation.cpp index 187f3ae..410135c 100644 --- a/gravity_compensation/src/gravity_compensation.cpp +++ b/gravity_compensation/src/gravity_compensation.cpp @@ -73,7 +73,9 @@ bool GravityCompensation::Compensate(const geometry_msgs::WrenchStamped &ft_zero { geometry_msgs::Vector3Stamped g; - g.vector = gravity.linear_acceleration; + g.vector.x = -gravity.linear_acceleration.x; // IMU will measure gravity in the opposite direction from F/T sensor, check https://github.com/kth-ros-pkg/force_torque_tools/pull/18 + g.vector.y = -gravity.linear_acceleration.y; + g.vector.z = -gravity.linear_acceleration.z; g.header = gravity.header; g.header.stamp = ros::Time();