Feature/implement grasp detector in grab #1239

P-ict0 · 2022-09-13T18:54:05Z

Merge after #1238

Implementation of active_grasp_detector in grab.py. Also added a retry.

robot_smach_states/src/robot_smach_states/manipulation/grab.py

MatthijsBurgh · 2022-09-15T09:13:53Z

robot_skills/src/robot_skills/arm/arms.py

@@ -155,6 +155,12 @@ def handover_detector(self):
        self._test_die(hasattr(self._arm, 'handover_detector'), "This arm does not have a handover_detector")
        return self._arm.handover_detector

+    @property
+    def gripper_position_detector(self):


What does a gripper_position_detector do? We already know where the gripper is with the joint_states/TF.

It reads the joint state of the gripper

So how open the fingers are?

Yes. And upon further inspection we can use the get_joint_states function of the robot object for this but this allows us to already specify which joint corresponds to the fingers. That is something that can potentially be improved. But for now we want to see if this gives us the desired behaviour. Lets not do premature optimization.

That is a different can of worms and not for this project.

The handover_detector can be ignored in this project, but gripper_position_detector should be modelled correctly. It doesn't mean it should be done correctly right now in this PR. But it should be done correctly soon.

#stopdeverloederingvanonzecode

We should create an issue about this. With a deadline this should be fixed.

I created an issue to look into the grasp position detector robot part #1248 . It does not have high priority though. So I don't think we need to add a deadline

PetervDooren

some minor remarks but looks like a promising start

robot_smach_states/src/robot_smach_states/manipulation/grab.py

Co-authored-by: PetervDooren <[email protected]>

P-ict0 · 2022-10-01T09:14:16Z

From testing, seems like an error from the kinect.

  File "example_grasping.py", line 46, in <module>
    grab_state.execute()

  File "/home/rodtu/ros/noetic/system/src/smach/src/smach/state_machine.py", line 381, in execute
    container_outcome = self._update_once()

  File "/home/rodtu/ros/noetic/system/src/smach/src/smach/state_machine.py", line 269, in _update_once
    raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " %
smach.exceptions.InvalidUserCodeError: Could not execute state 'PREPARE_GRASP' of type '<robot_smach_states.manipulation.grab.PrepareEdGrasp object at 0x7f178c0cf8e0>': Traceback (most recent call last):

  File "/home/rodtu/ros/noetic/system/src/smach/src/smach/state_machine.py", line 258, in _update_once
    outcome = self._current_state.execute(

  File "/home/rodtu/ros/noetic/system/src/robot_smach_states/src/robot_smach_states/manipulation/grab.py", line 55, in execute
    segm_res = self.robot.ed.update_kinect("%s" % entity.uuid)

  File "/home/rodtu/ros/noetic/system/src/robot_skills/src/robot_skills/world_model_ed.py", line 420, in update_kinect
    res = self._ed_kinect_update_srv(area_description=area_description,

  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 442, in __call__
    return self.call(*args, **kwds)

  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 498, in call
    request = rospy.msg.args_kwds_to_message(self.request_class, args, kwds)

  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/msg.py", line 105, in args_kwds_to_message
    return data_class(**kwds)

  File "/home/rodtu/ros/noetic/system/devel/lib/python3/dist-packages/ed_sensor_integration_msgs/srv/_Update.py", line 47, in __init__super(UpdateRequest, self).__init__(*args, **kwds)

  File "/opt/ros/noetic/lib/python3/dist-packages/genpy/message.py", line 361, in __init__
    raise AttributeError('%s is not an attribute of %s' % (k, self.__class__.__name__))

AttributeError: fit_supporting_entity is not an attribute of UpdateRequest```

PetervDooren · 2022-10-01T09:40:13Z

@P-ict0 The reason CI was failing was because the tests are done using the Mockbot. so when we add a robot part to our hero python object we should also add it to the mockbot to remain compatible. I fixed this in b277700

…:tue-robotics/tue_robocup into feature/implement_grasp_detector_in_grab

PetervDooren

A few things we must watch out for in the future:
retrying grasp might not always be desired but we'll see when that becomes a problem.
Some magic numbers are very hero-specific. but as long as we keep it configurable we should be good.

PetervDooren · 2022-10-01T14:21:38Z

robot_smach_states/src/robot_smach_states/manipulation/active_grasp_detector.py

@@ -14,7 +14,7 @@ class ActiveGraspDetector(smach.State):
    REQUIRED_ARM_PROPERTIES = {"required_gripper_types": [GripperTypes.GRASPING], }

    def __init__(self, robot: Robot, arm_designator: ArmDesignator, threshold_difference: float = 0.075,
-                 minimum_position: float = -0.82, max_torque: float = 0.15) -> None:
+                 minimum_position: float = -0.75, max_torque: float = 0.15) -> None:


These are still very much magic numbers. They have the downside that they need to be tuned constantly. But I think that may be for another time

Make this an issue. If we need keep changing this, this is something we need to work on with high priority.

I created an issue to look into the grasp position detector robot part #1248 . It does not have high priority though. So I don't think we need to add a deadline

stale

P-ict0 · 2022-10-01T14:43:42Z

A few things we must watch out for in the future: retrying grasp might not always be desired but we'll see when that becomes a problem. Some magic numbers are very hero-specific. but as long as we keep it configurable we should be good.

Yes, the retry option is defaulted to False for these cases. I think this is the correct way of approaching this.

MatthijsBurgh · 2022-10-01T15:05:59Z

robot_skills/src/robot_skills/arm/arms.py

@@ -155,6 +155,12 @@ def handover_detector(self):
        self._test_die(hasattr(self._arm, 'handover_detector'), "This arm does not have a handover_detector")
        return self._arm.handover_detector

+    @property
+    def gripper_position_detector(self):


We should create an issue about this. With a deadline this should be fixed.

MatthijsBurgh · 2022-10-01T15:08:07Z

robot_smach_states/src/robot_smach_states/manipulation/active_grasp_detector.py

@@ -14,7 +14,7 @@ class ActiveGraspDetector(smach.State):
    REQUIRED_ARM_PROPERTIES = {"required_gripper_types": [GripperTypes.GRASPING], }

    def __init__(self, robot: Robot, arm_designator: ArmDesignator, threshold_difference: float = 0.075,
-                 minimum_position: float = -0.82, max_torque: float = 0.15) -> None:
+                 minimum_position: float = -0.75, max_torque: float = 0.15) -> None:


Make this an issue. If we need keep changing this, this is something we need to work on with high priority.

robot_smach_states/src/robot_smach_states/manipulation/grab.py

PetervDooren · 2022-10-04T07:22:20Z

@MatthijsBurgh, your comments have been adressed. can we merge this PR now?

MatthijsBurgh · 2022-10-04T10:20:26Z

I have also created #1249. I think this is really important that we keep it structured how robot parts are added.

I sometimes get the feeling we are skipping the complex thoughts on how we keep the code organized for the sake of development. But in the end people are complaining code is messy, not robust, hard to maintain, not modular, etc. That is indeed what you get, when you skip the steps to keep your code organized.

I don't want to put a hold on development. So first get things working, then refactor it, so it is maintainable, etc. But this should be done before merging stuff into master. Because when it is in master, there is no motivation anymore to fix it.

So I am not sure, I accept this code in master yet. Lets have a discussion with everyone tonight how we want to balance between development and good, modular, robust, maintainable, etc. code.

GustavoDCC · 2022-10-04T16:36:13Z

From testing, I still have this error after add gripperpositiondetector to mockbot.

Traceback (most recent call last):
  File "/home/gustavo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 258, in _update_once
    outcome = self._current_state.execute(
  File "/home/gustavo/ros/noetic/system/src/robot_smach_states/src/robot_smach_states/manipulation/grab.py", line 55, in execute
    segm_res = self.robot.ed.update_kinect("%s" % entity.uuid)
  File "/home/gustavo/ros/noetic/system/src/robot_skills/src/robot_skills/world_model_ed.py", line 420, in update_kinect
    res = self._ed_kinect_update_srv(area_description=area_description,
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 442, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 498, in call
    request = rospy.msg.args_kwds_to_message(self.request_class, args, kwds) 
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/msg.py", line 105, in args_kwds_to_message
    return data_class(**kwds)
  File "/home/gustavo/ros/noetic/system/devel/lib/python3/dist-packages/ed_sensor_integration_msgs/srv/_Update.py", line 47, in __init__
    super(UpdateRequest, self).__init__(*args, **kwds)
  File "/opt/ros/noetic/lib/python3/dist-packages/genpy/message.py", line 361, in __init__
    raise AttributeError('%s is not an attribute of %s' % (k, self.__class__.__name__))
AttributeError: fit_supporting_entity is not an attribute of UpdateRequest

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "example_grasping.py", line 46, in <module>
    grab_state.execute()
  File "/home/gustavo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 381, in execute
    container_outcome = self._update_once()
  File "/home/gustavo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 269, in _update_once
    raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " %
smach.exceptions.InvalidUserCodeError: Could not execute state 'PREPARE_GRASP' of type '<robot_smach_states.manipulation.grab.PrepareEdGrasp object at 0x7fad4477eca0>': Traceback (most recent call last):
  File "/home/gustavo/ros/noetic/system/src/smach/src/smach/state_machine.py", line 258, in _update_once
    outcome = self._current_state.execute(
  File "/home/gustavo/ros/noetic/system/src/robot_smach_states/src/robot_smach_states/manipulation/grab.py", line 55, in execute
    segm_res = self.robot.ed.update_kinect("%s" % entity.uuid)
  File "/home/gustavo/ros/noetic/system/src/robot_skills/src/robot_skills/world_model_ed.py", line 420, in update_kinect
    res = self._ed_kinect_update_srv(area_description=area_description,
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 442, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 498, in call
    request = rospy.msg.args_kwds_to_message(self.request_class, args, kwds)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/msg.py", line 105, in args_kwds_to_message
    return data_class(**kwds)
  File "/home/gustavo/ros/noetic/system/devel/lib/python3/dist-packages/ed_sensor_integration_msgs/srv/_Update.py", line 47, in __init__
    super(UpdateRequest, self).__init__(*args, **kwds)
  File "/opt/ros/noetic/lib/python3/dist-packages/genpy/message.py", line 361, in __init__
    raise AttributeError('%s is not an attribute of %s' % (k, self.__class__.__name__))
AttributeError: fit_supporting_entity is not an attribute of UpdateRequest

robot_skills/src/robot_skills/arm/gripper_position_detector.py

…detector

… active_grasp_detector

P-ict0 requested a review from PetervDooren September 13, 2022 18:55

MatthijsBurgh previously requested changes Sep 13, 2022

View reviewed changes

robot_smach_states/src/robot_smach_states/manipulation/grab.py Outdated Show resolved Hide resolved

robot_smach_states/src/robot_smach_states/manipulation/grab.py Outdated Show resolved Hide resolved

MatthijsBurgh force-pushed the feature/implement_grasp_detector_in_grab branch from c2741bd to b3ead88 Compare September 13, 2022 20:33

MatthijsBurgh reviewed Sep 15, 2022

View reviewed changes

P-ict0 and others added 6 commits September 27, 2022 21:01

Added grasp detection after grab and retry grab

6642e75

Fixed typo in state machine

802f766

Fixed underscore

0d35c55

Add position detector

2be6ad1

change min position after testing

6d53e0f

Improved import

1461fa4

P-ict0 force-pushed the feature/implement_grasp_detector_in_grab branch from db06a2c to 1461fa4 Compare September 27, 2022 19:08

P-ict0 added 2 commits September 27, 2022 21:13

Added outcome object_not_grasped of Grab

f79701c

Added outcome to manipulate_machine

78798df

PetervDooren reviewed Sep 28, 2022

View reviewed changes

robot_smach_states/src/robot_smach_states/manipulation/grab.py Outdated Show resolved Hide resolved

robot_smach_states/src/robot_smach_states/manipulation/grab.py Outdated Show resolved Hide resolved

P-ict0 and others added 5 commits October 1, 2022 10:03

Go to failure if retry fails

97150dd

Co-authored-by: PetervDooren <[email protected]>

Added type hinting

e86de40

Completed type hinting

c466380

Added retry parameter and announcements from robot

66232e5

Added outcome to cleanup

8c17a90

add gripperpositiondetector to mockbot

b277700

P-ict0 added 3 commits October 1, 2022 13:28

Changed value after testing

7d91602

Fixed retry

a763de0

Merge branch 'feature/implement_grasp_detector_in_grab' of github.com…

b56e24f

…:tue-robotics/tue_robocup into feature/implement_grasp_detector_in_grab

P-ict0 requested a review from PetervDooren October 1, 2022 11:59

add outcome for the object not grasped

5bacddf

PetervDooren previously approved these changes Oct 1, 2022

View reviewed changes

MatthijsBurgh self-requested a review October 1, 2022 15:04

MatthijsBurgh requested changes Oct 1, 2022

View reviewed changes

PetervDooren mentioned this pull request Oct 4, 2022

grasp position detector: move robot specific parameters to robotpart attributes instead of arguments. #1248

Open

MatthijsBurgh mentioned this pull request Oct 4, 2022

handover_detector & gripper_position_detector should be part of the gripper, not the arm #1249

Open

Add value -0.75 to grasp

f31614e

GustavoDCC dismissed PetervDooren’s stale review via 4c078d1 October 11, 2022 19:14

MatthijsBurgh requested changes Oct 16, 2022

View reviewed changes

robot_skills/src/robot_skills/arm/gripper_position_detector.py Outdated Show resolved Hide resolved

robot_skills/src/robot_skills/arm/gripper_position_detector.py Show resolved Hide resolved

GustavoDCC force-pushed the feature/implement_grasp_detector_in_grab branch from b232dd7 to f31614e Compare October 16, 2022 13:41

GustavoDCC and others added 5 commits October 16, 2022 15:53

Add attribute and old docstring minimum_position to gripper_position_…

dca7493

…detector

Add attribute minimum_position to GripperPositionDetector in mockbot.py

b893518

Add minimum_position from the gripper_position_detector to compare in…

7631ff9

… active_grasp_detector

(grab.py) Added docs

c6d2d77

(temp) change retry to true

a6b42fe

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Feature/implement grasp detector in grab #1239

Feature/implement grasp detector in grab #1239

P-ict0 commented Sep 13, 2022

MatthijsBurgh Sep 15, 2022

PetervDooren Sep 15, 2022

MatthijsBurgh Sep 15, 2022

P-ict0 Sep 15, 2022

PetervDooren Sep 15, 2022

PetervDooren Sep 15, 2022

MatthijsBurgh Sep 15, 2022

MatthijsBurgh Sep 15, 2022

MatthijsBurgh Oct 1, 2022

PetervDooren Oct 4, 2022 •

edited

Loading

PetervDooren left a comment •

edited

Loading

P-ict0 commented Oct 1, 2022 •

edited

Loading

PetervDooren commented Oct 1, 2022

PetervDooren left a comment

PetervDooren Oct 1, 2022

MatthijsBurgh Oct 1, 2022

PetervDooren Oct 4, 2022

P-ict0 commented Oct 1, 2022

MatthijsBurgh Oct 1, 2022

MatthijsBurgh Oct 1, 2022

PetervDooren commented Oct 4, 2022

MatthijsBurgh commented Oct 4, 2022

GustavoDCC commented Oct 4, 2022 •

edited by MatthijsBurgh

Loading

Feature/implement grasp detector in grab #1239

Are you sure you want to change the base?

Feature/implement grasp detector in grab #1239

Conversation

P-ict0 commented Sep 13, 2022

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

PetervDooren Oct 4, 2022 • edited Loading

Choose a reason for hiding this comment

PetervDooren left a comment • edited Loading

Choose a reason for hiding this comment

P-ict0 commented Oct 1, 2022 • edited Loading

PetervDooren commented Oct 1, 2022

PetervDooren left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

P-ict0 commented Oct 1, 2022

Choose a reason for hiding this comment

Choose a reason for hiding this comment

PetervDooren commented Oct 4, 2022

MatthijsBurgh commented Oct 4, 2022

GustavoDCC commented Oct 4, 2022 • edited by MatthijsBurgh Loading

PetervDooren Oct 4, 2022 •

edited

Loading

PetervDooren left a comment •

edited

Loading

P-ict0 commented Oct 1, 2022 •

edited

Loading

GustavoDCC commented Oct 4, 2022 •

edited by MatthijsBurgh

Loading