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();