how to use the gravity compensation

[hxqup]

when i test the gravity compensation as the ros wiki instructed,but i found no changes has happend,all of the topics have the same values.

[anubhav-dogra]

@hxqup same here, then I made the following changes, now its working fine,

void NetftUtils::netftCallback(const geometry_msgs::WrenchStamped::ConstPtr& data)
{
  // Filter data
  std::vector<double> tempData;
  tempData.resize(6);
  tempData.at(0) = -data->wrench.force.x;
  tempData.at(1) = data->wrench.force.y;
  tempData.at(2) = data->wrench.force.z;
  tempData.at(3) = -data->wrench.torque.x;
  tempData.at(4) = data->wrench.torque.y;
  tempData.at(5) = data->wrench.torque.z;

  if(isFilterOn && !newFilter)
    lp->update(tempData,tempData);

  // Copy tool frame data. apply negative to x data to follow right hand rule convention (ft raw data does not)
  raw_data_tool.header.stamp = data->header.stamp;
  raw_data_tool.header.frame_id = ft_frame;
  raw_data_tool.wrench.force.x = tempData.at(0);
  raw_data_tool.wrench.force.y = tempData.at(1);
  raw_data_tool.wrench.force.z = tempData.at(2);
  raw_data_tool.wrench.torque.x = tempData.at(3);
  raw_data_tool.wrench.torque.y = tempData.at(4);
  raw_data_tool.wrench.torque.z = tempData.at(5);

  // Copy in new netft data in tool frame and transform to world frame
  transformFrame(raw_data_tool, raw_data_world, 'w');

  copyWrench(raw_data_world, tf_data_world, zero_wrench);
  copyWrench(raw_data_tool, tf_data_tool, zero_wrench);

  if (isGravityBiased) // Compensate for gravity. Assumes world Z-axis is up
  {
      // Gravity moment = (payloadLeverArm) cross (payload force)  <== all in the sensor frame. Need to convert to world later
      // Since it's assumed that the CoM of the payload is on the sensor's central axis, this calculation is simplified.
      double gravMomentX = -payloadLeverArm*tf_data_tool.wrench.force.y;
      double gravMomentY = payloadLeverArm*tf_data_tool.wrench.force.x;
      // Gravity moment = (payloadLeverArm) cross (payload force)
      // Subtract the gravity torques from the previously-calculated wrench in the tool frame
      tf_data_tool.wrench.torque.x = tf_data_tool.wrench.torque.x - gravMomentX;
      tf_data_tool.wrench.torque.y = tf_data_tool.wrench.torque.y - gravMomentY;

      // Convert to world to account for the gravity force. Assumes world-Z is up.
      //ROS_INFO_STREAM("gravity force in the world Z axis: "<< payloadWeight);
      transformFrame(tf_data_tool, tf_data_world, 'w');
      tf_data_world.wrench.force.z = tf_data_world.wrench.force.z - payloadWeight;

      // tf_data_world now accounts for gravity completely. Convert back to the tool frame to make that data available, too
      transformFrame(tf_data_world, tf_data_tool, 't');
  }

  if (isBiased) // Apply the bias for a static sensor frame
  {
    // Get tool bias in world frame
    geometry_msgs::WrenchStamped world_bias;
    transformFrame(bias, world_bias, 'w');
    // Add bias and apply threshold to get transformed data
    copyWrench(raw_data_world, tf_data_world, world_bias);
  }
  // else // Just pass the data straight through
  // {
  //   copyWrench(raw_data_world, tf_data_world, zero_wrench);
  //   copyWrench(raw_data_tool, tf_data_tool, zero_wrench);
  // }             

  // Apply thresholds
  applyThreshold(tf_data_world.wrench.force.x, threshold.wrench.force.x);
  applyThreshold(tf_data_world.wrench.force.y, threshold.wrench.force.y);
  applyThreshold(tf_data_world.wrench.force.z, threshold.wrench.force.z);
  applyThreshold(tf_data_world.wrench.torque.x, threshold.wrench.torque.x);
  applyThreshold(tf_data_world.wrench.torque.y, threshold.wrench.torque.y);
  applyThreshold(tf_data_world.wrench.torque.z, threshold.wrench.torque.z);
  applyThreshold(tf_data_tool.wrench.force.x, threshold.wrench.force.x);
  applyThreshold(tf_data_tool.wrench.force.y, threshold.wrench.force.y);
  applyThreshold(tf_data_tool.wrench.force.z, threshold.wrench.force.z);
  applyThreshold(tf_data_tool.wrench.torque.x, threshold.wrench.torque.x);
  applyThreshold(tf_data_tool.wrench.torque.y, threshold.wrench.torque.y);
  applyThreshold(tf_data_tool.wrench.torque.z, threshold.wrench.torque.z);
  //ROS_INFO_STREAM("Callback time: " << tf_data_tool.header.stamp.toSec()-ros::Time::now().toSec());
}