Closed hmn-ocado closed 5 years ago
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))
somehow the alternative_behavior
does not have a field pre_grasp
but pre_approach
. Where can this naming difference come from?
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).
somehow the
alternative_behavior
does not have a fieldpre_grasp
butpre_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
We observe a strange behavior, for surface grasp with tub_feasibility module between
pregrasp
andgodown
. The Joint trajectory ofgodown
start differently then thepregrasp
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 forgodown
?
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)
thanks for the help @JannisW we fixed it the same way for Ocado
We observe a strange behavior, for surface grasp with tub_feasibility module between
pregrasp
andgodown
. The Joint trajectory ofgodown
start differently then thepregrasp
which manifest as a fast jump.Where should we check that the
pregrasp
trajectory joint config is used forgodown
?