Gripper action not stalling in sim (ros ticket #4142)

[ahendrix]

The gripper action doesn't return stall in simulation because the simulation velocity oscillates and never settles. The proposed solution is to make "stalling" determined by position. The following code needs to change:

{{{
// Determines if the gripper has stalled.
if (fabs(msg->process_value_dot) > stall_velocity_threshold_)
{
last_movement_time_ = ros::Time::now();
}
}}}

The gripper action should keep a best_achieved_position_ and best_achieved_position_time_, and check if they've been surpassed recently.

trac data:

• Owner: mwise
• Reporter: sglaser
• Reported at: Wed Apr 7 16:42:43 2010
• URL: https://code.ros.org/trac/wg-ros-pkg/ticket/4142

[ahendrix]

[mwise] my working diff

{{{

Index: src/pr2_gripper_action.cpp

--- src/pr2_gripper_action.cpp (revision 35097)
+++ src/pr2_gripper_action.cpp (working copy)
@@ -53,6 +53,7 @@

 pn.param("goal_threshold", goal_threshold_, 0.01);
 pn.param("stall_velocity_threshold", stall_velocity_threshold_, 1e-6);

• pn.param("stall_motion_threshold", stall_motion_threshold_, 1e-3);
  pn.param("stall_timeout", stall_timeout_, 0.1);

pub_controller_command_ =
@@ -85,6 +86,7 @@
double min_error_seen_;
double goal_threshold_;
double stall_velocity_threshold_;

• double stall_motion_threshold_;
  double stall_timeout_;
  ros::Time last_movement_time_;

@@ -163,6 +165,7 @@
void controllerStateCB(const pr2_controllers_msgs::JointControllerStateConstPtr &msg)
{
last_controller_state_ = msg;

• static double last_position = 9999.99;
  ros::Time now = ros::Time::now();

if (!has_active_goal_)
@@ -207,7 +210,7 @@
else
{
// Determines if the gripper has stalled.

•  if (fabs(msg->process_value_dot) > stall_velocity_threshold_)
  

•  if (fabs(msg->process_value-last_position) > stall_motion_threshold_)
  

  {
  last_movement_time_ = ros::Time::now();
  }
  @@ -220,7 +223,9 @@
  active_goal_.setAborted(result);
  has_active_goal_ = false;
  }

•  last_position = msg->process_value;
  

  }
  +
  active_goal_.publishFeedback(feedback);
  }
  };

}}}

[ahendrix]

[mwise] i think this was fixed?

[ahendrix]

[hsu] a '''stall_velocity_threshold''' parameter was added, but not documented, I just updated documentation to reflect the code: [http://www.ros.org/wiki/pr2_gripper_action pr2_gripper_action]].