Action client not preempting action

asked 2019-02-21 08:07:48 -0600

hpoleselo gravatar image

Hello, i think I'm aware how to preempt an action, which is by using the method cancel_goal() from SimpleActionClient but maybe I'm missing some internal process from Actions... What I'm trying to achieve with the robot is just a pick and place application, but the problem is currently on placing the grasped object, because when i give the goal to grasp the object, i.e going to a certain goal (width) he's never gonna achieve it simply because to do so the robot would have to break the object... In this case the action i'm using doesn't have force feedback so I'm just trying to preempt the action after grasping the object but it's not happening. And the key thing is: i cannot open the gripper after grasping the object because the Action Server is busy trying to reach the goal that i gave before, hence cancelling the action would be my solution but currently not working. Am i missing something?

The code for closing the gripper:

def close_gripper(self):
        """ Closes the gripper using an action client. """
        gripper_goal = MoveGoal
        gripper_goal.speed = 0.04
        gripper_goal.width = 0.004
        self.client.send_goal(gripper_goal)
        time.sleep(0.5)
        self.client.cancel_goal()

Now the results: both results are running the same code, in the first case we achieve to grasp the object but then the gripper doesn't open. In the second case the robot doesn't grasp the object, meaning it achieves the goal, hence it can preempt the goal (which doesn't much sense to me because it's only preempting when the goal is finished when it should preempt regardless the actual goal is, right?)

When the gripper grasps the object: (no preempting happening after the gripper is closed)

[INFO] [1550753604.064094]: Detected pose stored. Sending it to the planner...
[INFO] [1550753606.068421]: Gripper opened.
[INFO] [1550753608.069608]: Going to the set down grasping pose...
Displaying the trajectory on RVIZ...
Displaying the trajectory on RVIZ...
[INFO] [1550753634.557037]: Gripper closed.
Displaying the trajectory on RVIZ...

When the gripper doesn't grasp the object the preempting is activated, meaning it will then open

[INFO] [1550753225.496508]: Detected pose stored. Sending it to the planner...
[INFO] [1550753227.496069]: Gripper opened.
[INFO] [1550753229.500993]: Going to the set down grasping pose...
Displaying the trajectory on RVIZ...
Displaying the trajectory on RVIZ...
[INFO] [1550753255.889914]: Gripper closed.
[ERROR] [1550753257.974755]: Received comm state PREEMPTING when in simple state DONE with SimpleActionClient in NS /franka_gripper/move
Displaying the trajectory on RVIZ...
[INFO] [1550753285.732524]: Gripper opened.
edit retag flag offensive close merge delete