Change sign of the gravity vector.

kth-ros-pkg · May 7, 2021 · 3ed307d · 3ed307d
1 parent 4affcef
commit 3ed307d
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 2 deletions.
diff --git 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
@@ -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();