feasibility check - surface grasp trajectory connection

[hmn-ocado]

We observe a strange behavior, for surface grasp with tub_feasibility module between pregrasp and godown. The Joint trajectory of godown start differently then the pregrasp which manifest as a fast jump.

Where should we check that the pregrasp trajectory joint config is used for godown?

[hmn-ocado]

pregrasp did not require sampling in this case, here is the log: Why is there a NEW START created for the godown?

check feasibility result was: 1
[/planner] [WARN] [776.823000]: Initial configuration [ 1.85531689e-05  4.60559697e-06  1.42917475e-05 -1.68878097e+00
 -7.94619553e-06  1.44164324e+00 -4.65954442e-06] is first point in trajectory
('USE NORMAL. Start: ', (-0.11014335326598229, 0.30317773955220984, -0.15698089221324882, -1.6280653309791984, -0.07180364787630343, 0.7550920364646776, 0.2577035706702987))
('GOAL_POSE', position: 
  x: 0.6361896945038633
  y: -0.16949777256908163
  z: 0.05297098370255571
orientation: 
  x: -0.9729867910879707
  y: 0.2308607900192957
  z: -1.6008021828593755e-08
  w: 2.981395469355352e-09)
('INIT_CONF', (-0.11014335326598229, 0.30317773955220984, -0.15698089221324882, -1.6280653309791984, -0.07180364787630343, 0.7550920364646776, 0.2577035706702987))
('allowed', [type: 1
box_id: 0
constraint_name: ''
terminating: True
required: True, type: 2
box_id: 0
constraint_name: bottom
terminating: False
required: False])
Call check kinematics for go_down (SurfaceGrasp)
Goal:
[[ 8.93406590e-01 -4.49248998e-01  3.25277621e-08  6.36189695e-01]
 [-4.49248998e-01 -8.93406590e-01 -1.58953239e-09 -1.69497773e-01]
 [ 2.97746129e-08 -1.31929659e-08 -9.99999998e-01  5.29709837e-02]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
check feasibility result was: 2
[/planner] [WARN] [777.925000]: Initial configuration (-0.11014335326598229, 0.30317773955220984, -0.15698089221324882, -1.6280653309791984, -0.07180364787630343, 0.7550920364646776, 0.2577035706702987) is first point in trajectory
('FOUND ALTERNATIVE. New Start: ', (-0.6724066956998032, 0.6089677900090359, 0.30030397969280326, -1.8898206992790028, 0.1669334857211059, 0.2436389678942201, -0.6259247077198653))

[zweistein]

somehow the alternative_behavior does not have a field pre_grasp but pre_approach. Where can this naming difference come from?

[JannisW]

There was no sampling required for pre_grasp (from initial joint config to pre_grasp), but then from pre grasp to go_down there was sampling involved (I guess, joint limit or collision with thumb, hard to say without the output of the feasibility node) You can see that here:

check feasibility result was: 2

It means that there was indeed sampling by the feasibility checker (It is the return code by the feasibility checker call). So it makes sense that alternative behavior is generated. 'New Start' just means new start of the sequence that was tested (pre_grasp -> go_down).

If you think the node should have generated a trajectory anyway, you should also check it's output (and post it here).

[JannisW]

somehow the alternative_behavior does not have a field pre_grasp but pre_approach. Where can this naming difference come from?

This is probably a result from the merge in which at some point pre_grasp was renamed pre_approach in handarm_parameters and thus to avoid introducing the old name again (because of the new parameters) I had to change it in the feasibility node as well This is surprising, because I thought I already changed it, but it does not appear to be in the commit... Whatever, the fix is easy. Just change 'pre_grasp' to 'pre_approach' for alternative_behavior I'll also commit a fix in a second to the branch tub_ocado_merge_full

EDIT: 4fae5d77c160980ce304dd4a16290c0dbe5472ed fixes this for RBOHandRecipesWAM

[JannisW]

We observe a strange behavior, for surface grasp with tub_feasibility module between pregrasp and godown. The Joint trajectory of godown start differently then the pregrasp which manifest as a fast jump.

What do you mean by "start differently" of course they start differently one starts at the inital joint config and goes to the pre_grasp pose and the second one goes from pre_grasp pose in joint space to the go_down pose.

Or do you mean the pre_grasp trajectory ends differently than the go_down starts? This is actually explicitly checked in the __init__ function of the AlternativeBehavior class (see tub_feasibility_check_interface.py):

if np.allclose(init_conf, self.trajectory_steps[0]):
    rospy.logwarn("Initial configuration {0} is first point in trajectory".format(init_conf))
    # :1 = skip the initial position TODO remove if relative is used
    self.trajectory_steps = self.trajectory_steps[1:]

If it is exactly the same the first point in the trajectory is removed (hack for the WAM). If it is just a little off you can remove that check and see if it works better for you!

Where should we check that the pregrasp trajectory joint config is used for godown?

If you really want to see if a generated trajectory was used used or the original one, check the controller types of GoDown and PreGrasp (they should be either JointControlMode or InterpolatedHTransformControlMode) in the HA. It will also include the trajectory itself, so you can check the start/end points. If you want to do it in code, the alternative_behavior object has a get_trajectory method (See: RBOHandRecipesWAM)

[zweistein]

thanks for the help @JannisW we fixed it the same way for Ocado