From 6f1047035729eeeeca96bfff49189765031c43f4 Mon Sep 17 00:00:00 2001 From: Johannes Wortmann Date: Thu, 7 Mar 2019 15:54:29 +0100 Subject: [PATCH 1/8] added support of new feasibility_check interface. (table from edges) Preparation for edge grasp checks --- ec_grasp_planner/src/multi_object_params.py | 218 +++++++++++++++++--- 1 file changed, 188 insertions(+), 30 deletions(-) diff --git a/ec_grasp_planner/src/multi_object_params.py b/ec_grasp_planner/src/multi_object_params.py index 3ac5ec1..03d484d 100755 --- a/ec_grasp_planner/src/multi_object_params.py +++ b/ec_grasp_planner/src/multi_object_params.py @@ -30,6 +30,12 @@ def angle_between(v1, v2): return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0)) +class EnvironmentalConstraint: + def __init__(self, transform, label): + self.transform = transform + self.label = label + + class AlternativeBehavior: # TODO this class should be adapted if return value of the feasibility check changes (e.g. switch conditions) def __init__(self, feasibility_check_result, init_conf): @@ -205,11 +211,13 @@ def get_matching_ifco_wall(ifco_in_base_transform, ec_frame): return 'east' # TODO move that to a separate file? - def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_index, strategy, all_ec_frames, + def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_index, all_ecs, ifco_in_base_transform, handarm_params): + strategy = all_ecs[current_ec_index].label + ec_frame = all_ecs[current_ec_index].transform + object = objects[current_object_idx] - ec_frame = all_ec_frames[current_ec_index] object_params = self.data[object['type']][strategy] object_params['frame'] = object['frame'] @@ -220,9 +228,15 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in # The collisions that are allowed in message format per motion allowed_collisions = {} - # The initial joint configuration (goToView config) - curr_start_config = [0.457929, 0.295013, -0.232804, 2.0226, 0.0, 1.50907, 1.0] # TODO use line below! - # curr_start_config = rospy.get_param('planner_gui/robot_view_position') # TODO use current joint state instead? + # The initial joint configuration (goToView config) This is not used right know, since view->init is not checked + # curr_start_config = rospy.get_param('planner_gui/robot_view_position') # TODO check view->init config + + # The edges in the scene + edges = [e for e in all_ecs if e.label == "EdgeGrasp"] + + # The backup table frame that is used in case we don't create the table from edges + table_frame = None + table_pose = geometry_msgs.msg.Pose() # TODO maybe move the kinematic stuff to separate file @@ -238,19 +252,24 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in params = handarm_params['surface_grasp']['object'] # TODO needed ---------------------- - x_flip_transform = tra.concatenate_matrices( - tra.translation_matrix([0, 0, 0]), tra.rotation_matrix(math.radians(180.0), [1, 0, 0])) + #x_flip_transform = tra.concatenate_matrices( + # tra.translation_matrix([0, 0, 0]), tra.rotation_matrix(math.radians(180.0), [1, 0, 0])) - table_frame = ec_frame.dot(x_flip_transform) + # TODO needed ---------------------- + + #table_frame = ec_frame.dot(x_flip_transform) + table_frame = ec_frame # Since the surface grasp frame is at the object center we have to translate it in z direction table_frame[2, 3] = table_frame[2, 3] - object['bounding_box'].z / 2.0 + # Convert table frame to pose message + table_pose = multi_object_params.transform_to_pose_msg(table_frame) + #object_frame = object_params['frame'].dot(x_flip_transform) object_frame = object_params['frame'] print("DBG_FRAME 1", multi_object_params.transform_to_pose_msg(object_frame)) - # TODO needed ---------------------- goal_ = np.copy(object_frame) # we rotate the frame to align the hand with the long object axis and the table surface @@ -332,13 +351,13 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in 'go_down': [AllowedCollision(type=AllowedCollision.BOUNDING_BOX, box_id=current_object_idx, terminating=True, required=True), AllowedCollision(type=AllowedCollision.ENV_CONSTRAINT, - constraint_name='bottom', terminating=False)], + constraint_name='table', terminating=False)], # TODO also account for the additional object in a way? 'post_grasp_rot': [AllowedCollision(type=AllowedCollision.BOUNDING_BOX, box_id=current_object_idx, terminating=True), AllowedCollision(type=AllowedCollision.ENV_CONSTRAINT, - constraint_name='bottom', terminating=False)] + constraint_name='table', terminating=False)] } elif strategy == "WallGrasp": @@ -476,28 +495,159 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in } + elif strategy == "EdgeGrasp": # TODO get rid of code redundancy + + if object['type'] in handarm_params['edge_grasp']: + params = handarm_params['edge_grasp'][object['type']] + else: + params = handarm_params['edge_grasp']['object'] + + hand_transform = params['hand_transform'] + pre_approach_transform = params['pre_approach_transform'] + palm_edge_offset = params['palm_edge_offset'] + + # This is the frame of the edge we are going for + edge_frame = np.copy(ec_frame) + + # this is the EC frame. It is positioned like object and oriented to the edge? + initial_slide_frame = np.copy(edge_frame) + initial_slide_frame[:3, 3] = tra.translation_from_matrix(object_params['frame']) + # apply hand transformation + # hand on object fingers pointing toward the edge + initial_slide_frame = (initial_slide_frame.dot(hand_transform)) + + # the pre-approach pose should be: + # - floating above the object + # -- position above the object depend the relative location of the edge to the object and the object bounding box + # TODO define pre_approach_transform(egdeTF, objectTF, objectBB) + # - fingers pointing toward the edge (edge x-axis orthogonal to palm frame x-axis) + # - palm normal perpendicualr to surface normal + pre_approach_pose = initial_slide_frame.dot(pre_approach_transform) + + # down_dist = params['down_dist'] # dist lower than ifco bottom: behavior of the high level planner + # dist = z difference to object centroid (both transformations are w.r.t. to world frame + # (more realistic behavior since we have to touch the object for a successful grasp) + down_dist = pre_approach_pose[2, 3] - object_params['frame'][2, 3] # get z-translation difference + + # goal pose for go down movement + go_down_pose = tra.translation_matrix([0, 0, -down_dist]).dot(pre_approach_pose) + + # goal pose for sliding + # 4. Go towards the edge to slide object to edge + # this is the tr from initial_slide_frame to edge frame in initial_slide_frame + delta = np.linalg.inv(edge_frame).dot(initial_slide_frame) + + # this is the distance to the edge, given by the z axis of the edge frame + # add some extra forward distance to avoid grasping the edge of the table palm_edge_offset + dist = delta[2, 3] + np.sign(delta[2, 3]) * palm_edge_offset + + # handPalm pose on the edge right before grasping + hand_on_edge_pose = initial_slide_frame.dot(tra.translation_matrix([dist, 0, 0])) + + # direction toward the edge and distance without any rotation in worldFrame + dir_edge = np.identity(4) + # relative distance from initial slide position to final hand on the edge position + distance_to_edge = hand_on_edge_pose - initial_slide_frame + # TODO might need tuning based on object and palm frame + dir_edge[:3, 3] = tra.translation_from_matrix(distance_to_edge) + # no lifting motion applied while sliding + dir_edge[2, 3] = 0 + + slide_to_edge_pose = dir_edge.dot(go_down_pose) + + checked_motions = ['pre_grasp', 'go_down', + 'slide_to_edge'] # TODO overcome problem of FT-Switch after go_down + + goals = [pre_approach_pose, go_down_pose, slide_to_edge_pose] # TODO see checked_motions + + # Take orientation of object but translation of pre grasp pose + pre_grasp_pos_manifold = np.copy(object_params['frame']) + pre_grasp_pos_manifold[:3, 3] = tra.translation_from_matrix(pre_approach_pose) + + slide_pos_manifold = np.copy(slide_to_edge_pose) + + goal_manifold_frames = { + 'pre_grasp': pre_grasp_pos_manifold, + + # Use object frame for sampling + 'go_down': np.copy(go_down_pose), + + # Use wall frame for sampling. Keep in mind that the wall frame has different orientation, than world. + 'slide_to_edge': slide_pos_manifold, + } + + goal_manifold_orientations = { + # use hand orientation + 'pre_grasp': tra.quaternion_from_matrix(pre_approach_pose), + + # Use object orientation + 'go_down': tra.quaternion_from_matrix(go_down_pose), # TODO use hand orietation instead? + + # use wall orientation + 'slide_to_edge': tra.quaternion_from_matrix(wall_frame), + } + + # override initial robot configuration + # TODO also check gotToView -> params['initial_goal'] (requires forward kinematics, or change to op-space) + curr_start_config = params['initial_goal'] + + allowed_collisions = { + + # 'init_joint': [], + + # no collisions are allowed during going to pre_grasp pose + 'pre_grasp': [], + + # Only allow touching the bottom of the ifco + 'go_down': [AllowedCollision(type=AllowedCollision.ENV_CONSTRAINT, constraint_name='table', + terminating=False), + ], + + 'slide_to_edge': [ + # Allow all other objects to be touched as well + # (since hand will go through them in simulation) TODO desired behavior? + AllowedCollision(type=AllowedCollision.BOUNDING_BOX, box_id=obj_idx, + terminating=False, required=obj_idx == current_object_idx) + for obj_idx in range(0, len(objects)) + ] + + [ + AllowedCollision(type=AllowedCollision.ENV_CONSTRAINT, constraint_name='table', + terminating=False), + ], + + } + else: # TODO implement other strategies raise ValueError("Kinematics checks are currently only supported for surface grasps and wall grasps, " "but strategy was " + strategy) - #return 0 # initialize stored trajectories for the given object self.stored_trajectories[(current_object_idx, current_ec_index)] = {} - # The pose of the ifco (in base frame) in message format - # ifco_pose = multi_object_params.transform_to_pose_msg(tra.inverse_matrix(ifco_in_base_transform)) - # TODO this only works for SurfaceGrasp. EdgeGrasp needs a different table representation. + # Try to create the table from edges. As fall back use SurfaceGrasp frame (which might have a wrong orientation) + table_from_edges = True + if len(edges) < 2 and table_frame is None: - x_flip_transform = tra.concatenate_matrices( - tra.translation_matrix([0, 0, 0]), tra.rotation_matrix(math.radians(180.0), [1, 0, 0])) + table_from_edges = False + # The potential SurfaceGrasp frames that can be used as a backup for table frames + pot_table_frames = [e.transform for e in all_ecs if all_ecs.label == 'SurfaceGrasp'] - if strategy != "SurfaceGrasp": - rospy.logerr("This only works for surface grasp right now (since we have the table frame)") + if len(pot_table_frames) > 0: + table_frame = pot_table_frames[0] - table_pose = multi_object_params.transform_to_pose_msg(table_frame) # TODO This only works for surface grasp right now (since we have the table frame) - #table_pose = multi_object_params.transform_to_pose_msg(np.copy(ec_frame).dot(x_flip_transform)) # TODO remove copy - print("TABLE POSE", table_pose) + # Since the surface grasp frame is at the object center we have to translate it in z direction + table_frame[2, 3] = table_frame[2, 3] - object['bounding_box'].z / 2.0 + + # Convert table frame to pose message + table_pose = multi_object_params.transform_to_pose_msg(table_frame) + + else: + # TODO limitation of the feasibility checker which needs at least two edges to create a table + raise ValueError("Planning for {}, but not enough edges/table frames found".format(strategy)) + + if table_frame is not None: + print("TABLE POSE", table_pose) # The bounding boxes of all objects in message format bounding_boxes = [] @@ -547,7 +697,9 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in goal_manifold_orientation=goal_manifold_orientation, min_orientation_deltas=params[manifold_name]['min_orientation_deltas'], max_orientation_deltas=params[manifold_name]['max_orientation_deltas'], - allowed_collisions=allowed_collisions[motion] + allowed_collisions=allowed_collisions[motion], + edge_frames=edges, + table_from_edges=table_from_edges ) print("check feasibility result was: " + str(res.status)) @@ -612,18 +764,19 @@ def reset_kinematic_checks_information(self): # ------------------------------------------------------------- # # object-environment-hand based heuristic, q_value for grasping - def heuristic(self, current_object_idx, objects, current_ec_index, strategy, all_ec_frames, ifco_in_base_transform, handarm_params): + def heuristic(self, current_object_idx, objects, current_ec_index, all_ecs, ifco_in_base_transform, handarm_params): object = objects[current_object_idx] + strategy = all_ecs[current_ec_index].label - ec_frame = all_ec_frames[current_ec_index] + ec_frame = all_ecs[current_ec_index].transform object_params = self.data[object['type']][strategy] object_params['frame'] = object['frame'] feasibility_checker = rospy.get_param("feasibility_check/active", default="TUB") if feasibility_checker == 'TUB': feasibility_fun = partial(self.check_kinematic_feasibility, current_object_idx, objects, current_ec_index, - strategy, all_ec_frames, ifco_in_base_transform, handarm_params) + all_ecs, ifco_in_base_transform, handarm_params) elif feasibility_checker == 'Ocado': # TODO integrate ocado @@ -631,6 +784,7 @@ def heuristic(self, current_object_idx, objects, current_ec_index, strategy, all else: # Since we are in disney use case, only black list wall, but not regions. + all_ec_frames = [ec.transform for ec in all_ecs] feasibility_fun = partial(self.black_list_walls, current_ec_index, all_ec_frames, strategy) q_val = 1 @@ -726,15 +880,19 @@ def process_objects_ecs(self, objects, ecs, graph_in_base, ifco_in_base_transfor print("The given object {} has no parameters set in the yaml {}".format(o["type"], self.file_name)) return -1, -1 - all_ec_frames = [] + #all_ec_frames = [] + all_ecs = [] for j, ec in enumerate(ecs): - all_ec_frames.append(graph_in_base.dot(self.transform_msg_to_homogenous_tf(ec.transform))) + # all_ec_frames.append(graph_in_base.dot(self.transform_msg_to_homogenous_tf(ec.transform))) + all_ecs.append( + EnvironmentalConstraint(graph_in_base.dot(self.transform_msg_to_homogenous_tf(ec.transform)), + ec.label)) print("ecs:{}".format(graph_in_base.dot(self.transform_msg_to_homogenous_tf(ec.transform)))) - for j, ec in enumerate(ecs): + for curr_ec_idx, ec in enumerate(ecs): # the ec frame must be in the same reference frame as the object ec_frame_in_base = graph_in_base.dot(self.transform_msg_to_homogenous_tf(ec.transform)) - Q_matrix[i,j] = self.heuristic(i, objects, j, ec.label, all_ec_frames, ifco_in_base_transform, + Q_matrix[i,curr_ec_idx] = self.heuristic(i, objects, curr_ec_idx, all_ecs, ifco_in_base_transform, handarm_parameters) # print (" ** h_mx = {}".format(Q_matrix)) From 0fff392f46ca5450c04bb7b4d3ec9ecc5184db48 Mon Sep 17 00:00:00 2001 From: Johannes Wortmann Date: Thu, 7 Mar 2019 16:06:51 +0100 Subject: [PATCH 2/8] added missing parameters and vars --- ec_grasp_planner/src/handarm_parameters.py | 20 ++++++++++++++++++++ ec_grasp_planner/src/multi_object_params.py | 4 +--- 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/ec_grasp_planner/src/handarm_parameters.py b/ec_grasp_planner/src/handarm_parameters.py index c071347..5755937 100644 --- a/ec_grasp_planner/src/handarm_parameters.py +++ b/ec_grasp_planner/src/handarm_parameters.py @@ -348,6 +348,26 @@ def __init__(self, **kwargs): self['edge_grasp']['object']['slide_velocity'] = np.array([0.125, 0.03]) self['edge_grasp']['object']['palm_edge_offset'] = 0 + self['edge_grasp']['object']['pre_grasp_manifold'] = Manifold( + {'min_position_deltas': [-0.05, -0.05, -0.05], + 'max_position_deltas': [0.05, 0.05, 0.05], + 'min_orientation_deltas': [0, 0, 0], + 'max_orientation_deltas': [0, 0, 0], + }) + + self['edge_grasp']['object']['go_down_manifold'] = Manifold({'min_position_deltas': [-0.01, -0.04, -0.05], + 'max_position_deltas': [0.06, 0.04, 0.01], + 'min_orientation_deltas': [0, 0, 0], + 'max_orientation_deltas': [0, 0, 0] + }) + + self['edge_grasp']['object']['slide_to_edge_manifold'] = Manifold( + {'min_position_deltas': [-0.04, -0.04, -0.01], + 'max_position_deltas': [0.04, 0.04, 0.01], + 'min_orientation_deltas': [0, 0, 0], + 'max_orientation_deltas': [0, 0, 0] + }) + # EDGE GRASP # ---------------------------------------------------------------------------- diff --git a/ec_grasp_planner/src/multi_object_params.py b/ec_grasp_planner/src/multi_object_params.py index 03d484d..ac1bd9e 100755 --- a/ec_grasp_planner/src/multi_object_params.py +++ b/ec_grasp_planner/src/multi_object_params.py @@ -584,7 +584,7 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in 'go_down': tra.quaternion_from_matrix(go_down_pose), # TODO use hand orietation instead? # use wall orientation - 'slide_to_edge': tra.quaternion_from_matrix(wall_frame), + 'slide_to_edge': tra.quaternion_from_matrix(edge_frame), } # override initial robot configuration @@ -880,10 +880,8 @@ def process_objects_ecs(self, objects, ecs, graph_in_base, ifco_in_base_transfor print("The given object {} has no parameters set in the yaml {}".format(o["type"], self.file_name)) return -1, -1 - #all_ec_frames = [] all_ecs = [] for j, ec in enumerate(ecs): - # all_ec_frames.append(graph_in_base.dot(self.transform_msg_to_homogenous_tf(ec.transform))) all_ecs.append( EnvironmentalConstraint(graph_in_base.dot(self.transform_msg_to_homogenous_tf(ec.transform)), ec.label)) From c87a1bd77cbd4afbc2c77afb9a3f3f85ff17a9da Mon Sep 17 00:00:00 2001 From: Johannes Wortmann Date: Tue, 12 Mar 2019 17:11:08 +0100 Subject: [PATCH 3/8] full edge grasp support for feasibility checker (untested) --- ec_grasp_planner/src/planner.py | 208 ++++++++++++++++++++++---------- 1 file changed, 143 insertions(+), 65 deletions(-) diff --git a/ec_grasp_planner/src/planner.py b/ec_grasp_planner/src/planner.py index 01f77a1..c95d408 100755 --- a/ec_grasp_planner/src/planner.py +++ b/ec_grasp_planner/src/planner.py @@ -891,15 +891,15 @@ def create_wall_grasp(object_frame, support_surface_frame, wall_frame, handarm_p reference_frame="world", v_max=go_down_velocity)) - # 3b. Switch when force threshold is exceeded - control_sequence.append(ha.ForceTorqueSwitch('GoDown', - 'LiftHand', - goal=go_down_force, - norm_weights=np.array([0, 0, 1, 0, 0, 0]), - jump_criterion="THRESH_UPPER_BOUND", - goal_is_relative='1', - frame_id='world', - port='2')) + # 3b. Switch when force threshold is exceeded (in both cases joint trajectory or op-space control) + control_sequence.append(ha.ForceTorqueSwitch('GoDown', + 'LiftHand', + goal=go_down_force, + norm_weights=np.array([0, 0, 1, 0, 0, 0]), + jump_criterion="THRESH_UPPER_BOUND", + goal_is_relative='1', + frame_id='world', + port='2')) # 4. Lift upwards so the hand doesn't slide on table surface # TODO: remove 4 and 4b if hand should slide on surface OR set duration=0.0 (latter only if joint control not used) @@ -1052,8 +1052,8 @@ def create_wall_grasp(object_frame, support_surface_frame, wall_frame, handarm_p # ================================================================================================ def create_edge_grasp(object_frame, support_surface_frame, edge_frame, handarm_params, object_type, handover, - object_params): - #TODO add mass estimation + object_params, alternative_behavior): + # Get the parameters from the handarm_parameters.py file if object_type in handarm_params['edge_grasp']: params = handarm_params['edge_grasp'][object_type] @@ -1138,17 +1138,35 @@ def create_edge_grasp(object_frame, support_surface_frame, edge_frame, handarm_p epsilon=str(math.radians(7.)))) # 1. Go above the object - # TODO hand pose relative to object should depend on bounding box size and edge location - control_sequence.append( - ha.InterpolatedHTransformControlMode(pre_approach_pose, controller_name='GoAboveObject', goal_is_relative='0', - name="PreGrasp", - )) - # null_space_posture=False#use_null_space_posture, - # null_space_goal_is_relative='1', + if alternative_behavior is not None and 'pre_grasp' in alternative_behavior: + + # we can not use the initially generated plan, but have to include the result of the feasibility checks + pre_grasp_velocity = params['max_joint_velocity'] + goal_traj = alternative_behavior['pre_grasp'].get_trajectory() + + print("GOAL TRAJ PreGrasp", goal_traj) # TODO remove (output just for debugging) + control_sequence.append(ha.JointControlMode(goal_traj, name='PreGrasp', controller_name='GoAboveObject', + goal_is_relative='0', + v_max=pre_grasp_velocity, + # for the close trajectory points linear interpolation works best. + interpolation_type='linear')) - # 1b. Switch when hand reaches the goal pose - control_sequence.append(ha.FramePoseSwitch('PreGrasp', 'ReferenceMassMeasurement', controller='GoAboveObject', - epsilon='0.01')) + # 1b. Switch when hand reaches the goal configuration + control_sequence.append(ha.JointConfigurationSwitch('PreGrasp', 'ReferenceMassMeasurement', + controller='GoAboveObject', epsilon=str(math.radians(7.)))) + + else: + # TODO hand pose relative to object should depend on bounding box size and edge location + control_sequence.append( + ha.InterpolatedHTransformControlMode(pre_approach_pose, controller_name='GoAboveObject', + goal_is_relative='0', name="PreGrasp", + )) + # null_space_posture=False#use_null_space_posture, + # null_space_goal_is_relative='1', + + # 1b. Switch when hand reaches the goal pose + control_sequence.append(ha.FramePoseSwitch('PreGrasp', 'ReferenceMassMeasurement', controller='GoAboveObject', + epsilon='0.01')) # TODO add a time switch and short waiting time before the reference measurement is actually done? # 2. Reference mass measurement with empty hand (TODO can this be replaced by offline calibration?) @@ -1175,20 +1193,60 @@ def create_edge_grasp(object_frame, support_surface_frame, edge_frame, handarm_p # Instead the timeout will trigger giving the user an opportunity to notice the erroneous result in the GUI. # 3. Go down onto the object/table, in world frame - dirDown = tra.translation_matrix([0, 0, -down_dist]) - control_sequence.append( - ha.InterpolatedHTransformControlMode(dirDown, - controller_name='GoDown', - goal_is_relative='1', - name="GoDown", - reference_frame="world", - v_max=go_down_velocity)) - - # 3b. Switch when force threshold is exceeded - force = np.array([0, 0, downward_force, 0, 0, 0]) + go_down_force = np.array([0, 0, downward_force, 0, 0, 0]) # force threshold for guarded go down motion + + if alternative_behavior is not None and 'go_down' in alternative_behavior: + # we can not use the initially generated plan, but have to include the result of the feasibility checks + # Go down onto the object (joint controller + relative world frame motion) + + go_down_velocity = params['go_down_velocity'] + go_down_joint_velocity = params['max_joint_velocity'] + goal_traj = alternative_behavior['go_down'].get_trajectory() + + print("GOAL_TRAJ GoDown", goal_traj) # TODO remove (only for debugging) + control_sequence.append(ha.JointControlMode(goal_traj, name='GoDown', controller_name='GoDown', + goal_is_relative='0', + v_max=go_down_joint_velocity, + # for the close trajectory points linear interpolation works best. + interpolation_type='linear')) + + # Relative motion that ensures that the actual force/torque threshold is reached + control_sequence.append( + ha.InterpolatedHTransformControlMode(tra.translation_matrix([0, 0, -10]), + controller_name='GoDownFurther', + goal_is_relative='1', + name="GoDownFurther", + reference_frame="world", + v_max=go_down_velocity)) + + # Switch when goal is reached to trigger the relative go down further motion + control_sequence.append(ha.JointConfigurationSwitch('GoDown', 'GoDownFurther', controller='GoDown', + epsilon=str(math.radians(7.)))) + + # Force/Torque switch for the additional relative go down further + control_sequence.append(ha.ForceTorqueSwitch('GoDownFurther', + 'Pause_1', + goal=go_down_force, + norm_weights=np.array([0, 0, 1, 0, 0, 0]), + jump_criterion="THRESH_UPPER_BOUND", + goal_is_relative='1', + frame_id='world', + port='2')) + + else: + dirDown = tra.translation_matrix([0, 0, -down_dist]) + control_sequence.append( + ha.InterpolatedHTransformControlMode(dirDown, + controller_name='GoDown', + goal_is_relative='1', + name="GoDown", + reference_frame="world", + v_max=go_down_velocity)) + + # 3b. Switch when force threshold is exceeded (in both cases joint trajectory or op-space control) control_sequence.append(ha.ForceTorqueSwitch('GoDown', 'Pause_1', - goal=force, + goal=go_down_force, norm_weights=np.array([0, 0, 1, 0, 0, 0]), jump_criterion="THRESH_UPPER_BOUND", goal_is_relative='1', @@ -1204,38 +1262,58 @@ def create_edge_grasp(object_frame, support_surface_frame, edge_frame, handarm_p control_sequence.append(ha.TimeSwitch('Pause_1', 'SlideToEdge', duration=0.02)) # 4. Go towards the edge to slide object to edge - #this is the tr from initial_slide_frame to edge frame in initial_slide_frame - delta = np.linalg.inv(edge_frame).dot(initial_slide_frame) - - # this is the distance to the edge, given by the z axis of the edge frame - # add some extra forward distance to avoid grasping the edge of the table palm_edge_offset - dist = delta[2,3] + np.sign(delta[2,3])*palm_edge_offset - - # handPalm pose on the edge right before grasping - hand_on_edge_pose = initial_slide_frame.dot(tra.translation_matrix([dist, 0, 0])) - - # direction toward the edge and distance without any rotation in worldFrame - dirEdge = np.identity(4) - # relative distance from initial slide position to final hand on the edge position - distance_to_edge = hand_on_edge_pose - initial_slide_frame - # TODO might need tuning based on object and palm frame - dirEdge[:3, 3] = tra.translation_from_matrix(distance_to_edge) - # no lifting motion applied while sliding - dirEdge[2, 3] = 0 - - # slide direction is given by the x-axis of the edge - rviz_frames.append(hand_on_edge_pose) - control_sequence.append( - ha.InterpolatedHTransformControlMode(dirEdge, controller_name='SlideToEdge', goal_is_relative='1', - name="SlideToEdge", reference_frame="world", - v_max=slide_velocity)) - - # 4b. Switch when hand reaches the goal pose - control_sequence.append(ha.FramePoseSwitch('SlideToEdge', mode_name_hand_closing, - controller='SlideToEdge', - epsilon='0.01', - reference_frame="world", - goal_is_relative='1')) + + if alternative_behavior is not None and 'slide_to_edge' in alternative_behavior: + # TODO + slide_joint_velocity = params['slide_joint_velocity'] + goal_traj = alternative_behavior['slide_to_edge'].get_trajectory() + + print("GOAL_TRAJ SlideToEdge", goal_traj) # TODO remove (only for debugging) + + # Sliding motion in joint space + control_sequence.append(ha.JointControlMode(goal_traj, name='SlideToEdge', controller_name='SlideToEdge', + goal_is_relative='0', + v_max=slide_joint_velocity, + # for the close trajectory points linear interpolation works best. + interpolation_type='linear')) + + # Switch when goal is reached to trigger the relative slide further motion + control_sequence.append(ha.JointConfigurationSwitch('SlideToEdge', mode_name_hand_closing, + controller='SlideToEdge', epsilon=str(math.radians(7.)))) + + else: + # this is the tr from initial_slide_frame to edge frame in initial_slide_frame + delta = np.linalg.inv(edge_frame).dot(initial_slide_frame) + + # this is the distance to the edge, given by the z axis of the edge frame + # add some extra forward distance to avoid grasping the edge of the table palm_edge_offset + dist = delta[2,3] + np.sign(delta[2,3])*palm_edge_offset + + # handPalm pose on the edge right before grasping + hand_on_edge_pose = initial_slide_frame.dot(tra.translation_matrix([dist, 0, 0])) + + # direction toward the edge and distance without any rotation in worldFrame + dirEdge = np.identity(4) + # relative distance from initial slide position to final hand on the edge position + distance_to_edge = hand_on_edge_pose - initial_slide_frame + # TODO might need tuning based on object and palm frame + dirEdge[:3, 3] = tra.translation_from_matrix(distance_to_edge) + # no lifting motion applied while sliding + dirEdge[2, 3] = 0 + + # slide direction is given by the x-axis of the edge + rviz_frames.append(hand_on_edge_pose) + control_sequence.append( + ha.InterpolatedHTransformControlMode(dirEdge, controller_name='SlideToEdge', goal_is_relative='1', + name="SlideToEdge", reference_frame="world", + v_max=slide_velocity)) + + # 4b. Switch when hand reaches the goal pose + control_sequence.append(ha.FramePoseSwitch('SlideToEdge', mode_name_hand_closing, + controller='SlideToEdge', + epsilon='0.01', + reference_frame="world", + goal_is_relative='1')) # 5. Maintain contact while closing the hand if handarm_params['isForceControllerAvailable']: From dce8b06cbde55528147d200d3a8265ca5e51f448 Mon Sep 17 00:00:00 2001 From: Johannes Wortmann Date: Fri, 15 Mar 2019 15:15:43 +0100 Subject: [PATCH 4/8] fixed bug in success estimation (no clean separation between difference and absolute model) --- .../src/grasp_success_estimator.py | 101 +++++++++++++----- 1 file changed, 76 insertions(+), 25 deletions(-) diff --git a/ec_grasp_planner/src/grasp_success_estimator.py b/ec_grasp_planner/src/grasp_success_estimator.py index 72a5c9e..b6f0146 100755 --- a/ec_grasp_planner/src/grasp_success_estimator.py +++ b/ec_grasp_planner/src/grasp_success_estimator.py @@ -6,6 +6,7 @@ import tf.transformations from enum import Enum from collections import deque +from functools import partial import handarm_parameters @@ -34,6 +35,13 @@ class RESPONSES(Enum): GRASP_SUCCESS_ESTIMATOR_INACTIVE = 99.0 +class EstimationModes(Enum): + # Use a reference measurement (in a similar pose) and compare against that (more stable) + DIFFERENCE = 0 + # Just one absolute measurement. Might be more prone to robot noise/ calibration errors. + ABSOLUTE = 1 + + class ObjectModel(object): def __init__(self, name, mass_mean, mass_stddev): # The name of the object class (e.g. cucumber) @@ -110,7 +118,7 @@ def get_robot_noise_params(self): # return stored robot noise return self.robot_noise - def __init__(self, ft_topic_name, ft_topic_type, object_ros_param_path, path_to_object_parameters, ee_frame): + def __init__(self, ft_topic_name, ft_topic_type, object_ros_param_path, path_to_object_parameters, ee_frame, model): rospy.init_node('graspSuccessEstimatorMass', anonymous=True) self.tf_listener = tf.TransformListener() @@ -124,6 +132,12 @@ def __init__(self, ft_topic_name, ft_topic_type, object_ros_param_path, path_to_ self.robot_noise = None + # Check on what model the estimation should be based on + try: + self.model = EstimationModes[model.upper()] + except KeyError: + raise ValueError("IllegalArgument: MassEstimator model {} not supported".format(model)) + # This is the ft measurement that is used as the empty hand reference. From this one we can calculate change. self.ft_measurement_reference = None # The calculated reference mass in kg. @@ -277,17 +291,20 @@ def calculate_reference_mass(self): self.publish_status(RESPONSES.REFERENCE_MEASUREMENT_FAILURE) def estimate_number_of_objects(self): - if self.ft_measurement_reference is None: - rospy.logerr("MassEstimator: No reference measurement is present.") - self.publish_status(RESPONSES.ESTIMATION_RESULT_UNKNOWN_FAILURE) - return # failure - - if self.current_object_name is None or self.current_object_name != rospy.get_param(self.object_ros_param_path, - default=None): - rospy.logerr("MassEstimator: The object name of the reference measurement doesn't match the one of the " - "estimation.") - self.publish_status(RESPONSES.ESTIMATION_RESULT_UNKNOWN_FAILURE) - return # failure + + if self.model == EstimationModes.DIFFERENCE: + + if self.ft_measurement_reference is None: + rospy.logerr("MassEstimator: No reference measurement is present.") + self.publish_status(RESPONSES.ESTIMATION_RESULT_UNKNOWN_FAILURE) + return # failure + + if self.current_object_name is None or self.current_object_name != rospy.get_param(self.object_ros_param_path, + default=None): + rospy.logerr("MassEstimator: The object name of the reference measurement doesn't match the one of the " + "estimation.") + self.publish_status(RESPONSES.ESTIMATION_RESULT_UNKNOWN_FAILURE) + return # failure if self.current_object_name not in self.objects_info: @@ -300,29 +317,34 @@ def estimate_number_of_objects(self): current_ft_estimate = self.get_current_ft_estimation() if current_ft_estimate is not None: - second_mass = MassEstimator.force2mass(current_ft_estimate.force.z) # z-axis - mass_diff = second_mass - self.reference_mass + current_mass = MassEstimator.force2mass(current_ft_estimate.force.z) # z-axis + reference_mass = self.reference_mass if self.model == EstimationModes.DIFFERENCE else 0.0 + mass_diff = current_mass - reference_mass - print("MASSES:", second_mass, self.reference_mass, " diff:", mass_diff) + print("MASSES:", current_mass, reference_mass, " diff:", mass_diff, " model:", str(self.model)) print("FT_IN_BASE ref:", self.ft_measurement_reference) print("FT_IN_BASE est:", current_ft_estimate) - # basic object distribution (mean gets shifted by number of objects that are checked against) - object_mean = self.objects_info[self.current_object_name].mass_mean - object_stddev = self.objects_info[self.current_object_name].mass_stddev + current_object = self.objects_info[self.current_object_name] # robot specific parameters (used to check for no object) robot_noise = self.get_robot_noise_params() + if self.model == EstimationModes.DIFFERENCE: + pdf_fun = partial(MassEstimator.pdf_difference_model, reference_mass) + else: + pdf_fun = MassEstimator.pdf_absolute_model + # check for no object first (robot specific parameters) - max_pdf_val = norm.pdf(mass_diff, robot_noise.mass_mean, robot_noise.mass_stddev) + max_pdf_val = pdf_fun(0, current_mass, current_object, robot_noise) max_num_obj = 0 pdf_val_sum = max_pdf_val pdf_values = [max_pdf_val] # check for number of objects > 0 + # basic object distribution (mean gets shifted by number of objects that are checked against) for num_obj in range(1, 5): # classes (number of detected objects) we perform maximum likelihood on. - pdf_val = norm.pdf(mass_diff, object_mean * num_obj, object_stddev) + pdf_val = pdf_fun(num_obj, current_mass, current_object, robot_noise) pdf_values.append(pdf_val) pdf_val_sum += pdf_val if pdf_val > max_pdf_val: @@ -341,8 +363,7 @@ def estimate_number_of_objects(self): self.estimator_confidence_all_pub.publish(MassEstimator.list_to_Float64MultiArrayMsg(confidence_all)) # publish the estimated masses - self.estimator_mass_pub.publish(MassEstimator.list_to_Float64MultiArrayMsg( - [self.reference_mass, second_mass])) + self.estimator_mass_pub.publish(MassEstimator.list_to_Float64MultiArrayMsg([reference_mass, current_mass])) # publish the corresponding status message if max_num_obj == 0: @@ -358,14 +379,44 @@ def estimate_number_of_objects(self): rospy.logerr("MassEstimator: Estimation measurement couldn't be done. Is FT-topic alive?") self.publish_status(RESPONSES.ESTIMATION_RESULT_UNKNOWN_FAILURE) + # Probability density function for the difference model (Compare the mass difference between a reference measurement + # in a similar hand pose with a second one) + # + # IMPORTANT: To calculate the correct parameters set the create_absolute_distributions parameter in the calculation + # script (calculate_success_estimator_object_params.py, in soma_utils) to False. + @staticmethod + def pdf_difference_model(reference_mass, number_of_objects, current_mass, current_object, robot_noise): + + mass_diff = current_mass - reference_mass + + if number_of_objects == 0: + return norm.pdf(mass_diff, 0, robot_noise.mass_stddev) + + return norm.pdf(mass_diff, current_object.mass_mean * number_of_objects, current_object.mass_stddev) + + # Probability density function for the absolute model (No reference measurement is taken, instead the current + # mass measurement is directly used). + # + # IMPORTANT: This function assumes that the robot noise is already incorporated into the mass distribution of the + # object. To do so you can set the create_absolute_distributions parameter in the calculation script + # (calculate_success_estimator_object_params.py, in soma_utils) to True. + @staticmethod + def pdf_absolute_model(number_of_objects, current_mass, current_object, robot_noise): + + if number_of_objects == 0: + return norm.pdf(current_mass, robot_noise.mass_mean, robot_noise.mass_stddev) + + mass_mean = current_object.mass_mean * number_of_objects - robot_noise.mass_mean * (number_of_objects-1) + return norm.pdf(current_mass, mass_mean, current_object.mass_stddev) + if __name__ == '__main__': my_argv = rospy.myargv(argv=sys.argv) - if len(my_argv) < 6: - print("usage: grasp_success_estimator.py ft_topic_name ft_topic_type object_ros_param_path path_to_object_parameters ee_frame") + if len(my_argv) < 7: + print("usage: grasp_success_estimator.py ft_topic_name ft_topic_type object_ros_param_path path_to_object_parameters ee_frame mass_model") else: - we = MassEstimator(my_argv[1], my_argv[2], my_argv[3], my_argv[4], my_argv[5]) + we = MassEstimator(my_argv[1], my_argv[2], my_argv[3], my_argv[4], my_argv[5], my_argv[6]) rospy.spin() # locking between the callbacks not required as long as we use only one spinner. See: # https://answers.ros.org/question/48429/should-i-use-a-lock-on-resources-in-a-listener-node-with-multiple-callbacks/ From 33b5df2b42ff36fa82836910d92f43ecbb9edb9e Mon Sep 17 00:00:00 2001 From: Johannes Wortmann Date: Fri, 15 Mar 2019 15:16:49 +0100 Subject: [PATCH 5/8] fixed typos --- ec_grasp_planner/src/multi_object_params.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ec_grasp_planner/src/multi_object_params.py b/ec_grasp_planner/src/multi_object_params.py index ac1bd9e..01659fe 100755 --- a/ec_grasp_planner/src/multi_object_params.py +++ b/ec_grasp_planner/src/multi_object_params.py @@ -581,7 +581,7 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in 'pre_grasp': tra.quaternion_from_matrix(pre_approach_pose), # Use object orientation - 'go_down': tra.quaternion_from_matrix(go_down_pose), # TODO use hand orietation instead? + 'go_down': tra.quaternion_from_matrix(go_down_pose), # TODO use hand orientation instead? # use wall orientation 'slide_to_edge': tra.quaternion_from_matrix(edge_frame), @@ -598,7 +598,7 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in # no collisions are allowed during going to pre_grasp pose 'pre_grasp': [], - # Only allow touching the bottom of the ifco + # Only allow touching the table 'go_down': [AllowedCollision(type=AllowedCollision.ENV_CONSTRAINT, constraint_name='table', terminating=False), ], From fcb83f06a837e272e4f9475f1af767d6d1a40b5e Mon Sep 17 00:00:00 2001 From: Johannes Wortmann Date: Tue, 2 Apr 2019 20:56:25 +0200 Subject: [PATCH 6/8] fixed various frame issues for edge_grasp. This convention mismatch is a mess. now kind of working. TODO remove dbg and test --- ec_grasp_planner/src/multi_object_params.py | 122 ++++++++++++-------- ec_grasp_planner/src/planner.py | 8 +- ec_grasp_planner/src/planner_utils.py | 51 ++++++++ 3 files changed, 131 insertions(+), 50 deletions(-) create mode 100644 ec_grasp_planner/src/planner_utils.py diff --git a/ec_grasp_planner/src/multi_object_params.py b/ec_grasp_planner/src/multi_object_params.py index 01659fe..17e5e0c 100755 --- a/ec_grasp_planner/src/multi_object_params.py +++ b/ec_grasp_planner/src/multi_object_params.py @@ -11,6 +11,8 @@ from shape_msgs.msg import SolidPrimitive import rospy from functools import partial +import planner_utils as pu + import rospkg @@ -18,18 +20,6 @@ pkg_path = rospack.get_path('ec_grasp_planner') -def unit_vector(vector): - # Returns the unit vector of the vector. - return vector / np.linalg.norm(vector) - - -def angle_between(v1, v2): - # Returns the angle in radians between vectors 'v1' and 'v2':: - v1_u = unit_vector(v1) - v2_u = unit_vector(v2) - return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0)) - - class EnvironmentalConstraint: def __init__(self, transform, label): self.transform = transform @@ -119,7 +109,7 @@ def pdf_object_ec(self, object, ec_frame, strategy): for idx, val in enumerate(object['angle']): ec_x_axis = ec_frame[0:3, 0] angle_epsilon = object['epsilon'] - diff_angle = math.fabs(angle_between(obj_x_axis, ec_x_axis) - math.radians(val)) + diff_angle = math.fabs(pu.angle_between(obj_x_axis, ec_x_axis) - math.radians(val)) # print("obj_x = {}, ec_x = {}, eps = {}, optimalDeg = {}, copare = {}".format( # obj_x_axis, ec_x_axis, angle_epsilon, val, diff_angle)) if diff_angle <= math.radians(angle_epsilon): @@ -139,7 +129,7 @@ def pdf_object_ec(self, object, ec_frame, strategy): if strategy in ["WallGrasp", "EdgeGrasp"]: delta = np.linalg.inv(ec_frame).dot(object_frame) # this is the distance between object and EC - dist = delta[2, 3] + dist = abs(delta[2, 3]) # TODO why was this never a problem before? # include distance to q_val, longer distance decreases q_val q_val = q_val * (1/dist) @@ -147,14 +137,17 @@ def pdf_object_ec(self, object, ec_frame, strategy): def black_list_walls(self, current_ec_index, all_ec_frames, strategy): + print(":::A1") if strategy not in ["WallGrasp", "EdgeGrasp"]: return 1 # this function will blacklist all walls except # the one on th right side of the robot # y coord is the smallest + print(":::A2") if all_ec_frames[current_ec_index][1,3] > 0: - return 0 + print(":::A3") + return 0 min_y = 10000 min_y_index = 0 @@ -165,8 +158,10 @@ def black_list_walls(self, current_ec_index, all_ec_frames, strategy): min_y_index = i if min_y_index == current_ec_index: + print(":::A4") return 1 else: + print(":::A5") return 0 def black_list_unreachable_zones(self, object, object_params, ifco_in_base_transform, strategy): @@ -232,11 +227,13 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in # curr_start_config = rospy.get_param('planner_gui/robot_view_position') # TODO check view->init config # The edges in the scene - edges = [e for e in all_ecs if e.label == "EdgeGrasp"] + edges = [multi_object_params.transform_to_pose_msg(e.transform) for e in all_ecs if e.label == "EdgeGrasp"] # The backup table frame that is used in case we don't create the table from edges - table_frame = None - table_pose = geometry_msgs.msg.Pose() + #table_frame = None + #table_pose = geometry_msgs.msg.Pose() + table_frame = tra.inverse_matrix(ifco_in_base_transform) # ec_frame (does not have the correct orientation) + table_pose = multi_object_params.transform_to_pose_msg(table_frame) # TODO maybe move the kinematic stuff to separate file @@ -251,25 +248,17 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in else: params = handarm_params['surface_grasp']['object'] - # TODO needed ---------------------- - #x_flip_transform = tra.concatenate_matrices( - # tra.translation_matrix([0, 0, 0]), tra.rotation_matrix(math.radians(180.0), [1, 0, 0])) - - # TODO needed ---------------------- - - #table_frame = ec_frame.dot(x_flip_transform) - table_frame = ec_frame + table_frame = tra.inverse_matrix(ifco_in_base_transform) # ec_frame (does not have the correct orientation) # Since the surface grasp frame is at the object center we have to translate it in z direction - table_frame[2, 3] = table_frame[2, 3] - object['bounding_box'].z / 2.0 + #table_frame[2, 3] = table_frame[2, 3] - object['bounding_box'].z / 2.0 # Convert table frame to pose message table_pose = multi_object_params.transform_to_pose_msg(table_frame) - #object_frame = object_params['frame'].dot(x_flip_transform) object_frame = object_params['frame'] - print("DBG_FRAME 1", multi_object_params.transform_to_pose_msg(object_frame)) + #print("DBG_FRAME 1", multi_object_params.transform_to_pose_msg(object_frame)) goal_ = np.copy(object_frame) # we rotate the frame to align the hand with the long object axis and the table surface @@ -281,9 +270,19 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in goal_[:3, 1] = y_axis / np.linalg.norm(y_axis) goal_[:3, 2] = z_axis + print("DBG_FRAME 1", pu.tf_dbg_call_to_string(goal_, "dbg1")) + + # In the disney use case the z-axis points downwards, but the planner compensates for that already by + # flipping the support_surface_frame. Since the feasibilty_checker assumes the z-axis for the disney use + # case flipped as well, we need to apply the flip afterwards, when creating the pre-grasp pose + # TODO get rid of this mess (change convention in feasibility checker) + x_flip_transform = tra.concatenate_matrices( + tra.translation_matrix([0, 0, 0]), tra.rotation_matrix(math.radians(180.0), [1, 0, 0])) + goal_ = goal_.dot(x_flip_transform) + goal_ = goal_.dot(params['hand_transform']) - print("DBG_FRAME 2", multi_object_params.transform_to_pose_msg(goal_)) + print("DBG_FRAME 2", pu.tf_dbg_call_to_string(goal_, "dbg2")) # Set the initial pose above the object #goal_ = np.copy(object_params['frame']) # TODO: this should be support_surface_frame @@ -509,6 +508,18 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in # This is the frame of the edge we are going for edge_frame = np.copy(ec_frame) + # In the disney use case the z-axis of the table points downwards and the z-axis of the edge lays inside + # the plane, but the planner compensates for that already by rotating the edge_frame. + # Since the feasibilty_checker assumes the z-axis for the disney use + # case flipped as well, we need to apply the flip afterwards, when creating the pre-grasp pose + # TODO get rid of this mess (change convention in feasibility checker) + x_rot_transform = tra.concatenate_matrices( + tra.translation_matrix([0, 0, 0]), tra.rotation_matrix(math.radians(-90.0), [1, 0, 0])) + edge_frame = edge_frame.dot(x_rot_transform) + + print("EDGE_MULTI", pu.tf_dbg_call_to_string(edge_frame, "EDGE_MULTI")) + print("OBJ_MULTI", pu.tf_dbg_call_to_string(object_params['frame'], "OBJ_MULTI")) + # this is the EC frame. It is positioned like object and oriented to the edge? initial_slide_frame = np.copy(edge_frame) initial_slide_frame[:3, 3] = tra.translation_from_matrix(object_params['frame']) @@ -524,6 +535,8 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in # - palm normal perpendicualr to surface normal pre_approach_pose = initial_slide_frame.dot(pre_approach_transform) + print("PREEE", pu.tf_dbg_call_to_string(pre_approach_pose, "PRE1")) + # down_dist = params['down_dist'] # dist lower than ifco bottom: behavior of the high level planner # dist = z difference to object centroid (both transformations are w.r.t. to world frame # (more realistic behavior since we have to touch the object for a successful grasp) @@ -627,24 +640,28 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in # Try to create the table from edges. As fall back use SurfaceGrasp frame (which might have a wrong orientation) table_from_edges = True - if len(edges) < 2 and table_frame is None: + if len(edges) < 2: + # we use the SurfaceGrasp frame as a backup table_from_edges = False - # The potential SurfaceGrasp frames that can be used as a backup for table frames - pot_table_frames = [e.transform for e in all_ecs if all_ecs.label == 'SurfaceGrasp'] - if len(pot_table_frames) > 0: - table_frame = pot_table_frames[0] + # if table_frame is None: + # # we did not already compute a table pose backup... + # # Find potential SurfaceGrasp frames that can be used as a backup for table frames + # pot_table_frames = [e.transform for e in all_ecs if e.label == 'SurfaceGrasp'] + + # if len(pot_table_frames) > 0: + # table_frame = pot_table_frames[0] - # Since the surface grasp frame is at the object center we have to translate it in z direction - table_frame[2, 3] = table_frame[2, 3] - object['bounding_box'].z / 2.0 + # # Since the surface grasp frame is at the object center we have to translate it in z direction + # table_frame[2, 3] = table_frame[2, 3] - object['bounding_box'].z / 2.0 - # Convert table frame to pose message - table_pose = multi_object_params.transform_to_pose_msg(table_frame) + # Convert table frame to pose message + # table_pose = multi_object_params.transform_to_pose_msg(table_frame) - else: - # TODO limitation of the feasibility checker which needs at least two edges to create a table - raise ValueError("Planning for {}, but not enough edges/table frames found".format(strategy)) + # else: + # # TODO limitation of the feasibility checker which needs at least two edges to create a table + # raise ValueError("Planning for {}, but not enough edges/table frames found".format(strategy)) if table_frame is not None: print("TABLE POSE", table_pose) @@ -685,11 +702,11 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in print("allowed", allowed_collisions[motion]) - print("Call check kinematics for " + motion + " " + str(curr_goal))#Arguments: \n" + yaml.safe_dump(args)) + print("Call check kinematics for " + motion + " (" + strategy + ")\nGoal:\n" + str(curr_goal)) res = check_feasibility(initial_configuration=curr_start_config, goal_pose=goal_pose, - table_pose=table_pose, + table_surface_pose=table_pose, bounding_boxes_with_poses=bounding_boxes, goal_manifold_frame=goal_manifold_frame, min_position_deltas=params[manifold_name]['min_position_deltas'], @@ -730,8 +747,8 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in self.stored_trajectories[(current_object_idx, current_ec_index)] = {} pass - return self.pdf_object_strategy(object_params) * self.pdf_object_ec(object_params, ec_frame, - strategy) + #return self.pdf_object_strategy(object_params) * self.pdf_object_ec(object_params, ec_frame,strategy) + return 1 def black_list_risk_regions(self, current_object_idx, objects, current_ec_index, strategy, all_ec_frames, ifco_in_base_transform): @@ -787,13 +804,22 @@ def heuristic(self, current_object_idx, objects, current_ec_index, all_ecs, ifco all_ec_frames = [ec.transform for ec in all_ecs] feasibility_fun = partial(self.black_list_walls, current_ec_index, all_ec_frames, strategy) + print("---------------------") + print(self.pdf_object_strategy(object_params)) + print(self.pdf_object_ec(object_params, ec_frame, strategy)) + f = feasibility_fun() + print(f) + + print("---------------------") + q_val = 1 q_val = q_val * \ self.pdf_object_strategy(object_params) * \ self.pdf_object_ec(object_params, ec_frame, strategy) * \ - feasibility_fun() + f#feasibility_fun() - # print(" ** q_val = {} blaklisted={}".format(q_val, self.black_list_walls(current_ec_index, all_ec_frames))) + print("qval", q_val) + #print(" ** q_val = {} blaklisted={}".format(q_val, self.black_list_walls(current_ec_index, all_ec_frames))) return q_val ## --------------------------------------------------------- ## diff --git a/ec_grasp_planner/src/planner.py b/ec_grasp_planner/src/planner.py index c95d408..e3af220 100755 --- a/ec_grasp_planner/src/planner.py +++ b/ec_grasp_planner/src/planner.py @@ -59,6 +59,7 @@ import hatools.components as ha import hatools.cookbook as cookbook import tf_conversions.posemath as pm +import planner_utils as pu import handarm_parameters @@ -1106,6 +1107,9 @@ def create_edge_grasp(object_frame, support_surface_frame, edge_frame, handarm_p # if initial_slide_frame[0][0]<0: # goal_ = initial_slide_frame.dot(zflip_transform) + print("EDGE_PLN", pu.tf_dbg_call_to_string(edge_frame, "EDGE_PLN")) + print("OBJ_PLN", pu.tf_dbg_call_to_string(object_frame, "OBJ_PLN")) + # the pre-approach pose should be: # - floating above the object @@ -1509,9 +1513,9 @@ def hybrid_automaton_from_motion_sequence(motion_sequence, graph, T_robot_base_f print("Creating hybrid automaton for object {} and grasp type {}.".format(object_type, grasp_type)) if grasp_type == 'EdgeGrasp': support_surface_frame_node = get_node_from_actions(motion_sequence, 'move_object', graph) - support_surface_frame = T_robot_base_frame.dot(transform_msg_to_homogenous_tf(support_surface_frame_node.transform)) + support_surface_frame = T_robot_base_frame.dot(pu.convert_transform_msg_to_homogeneous_tf(support_surface_frame_node.transform)) edge_frame_node = get_node_from_actions(motion_sequence, 'grasp_object', graph) - edge_frame = T_robot_base_frame.dot(transform_msg_to_homogenous_tf(edge_frame_node.transform)) + edge_frame = T_robot_base_frame.dot(pu.convert_transform_msg_to_homogeneous_tf(edge_frame_node.transform)) #we need to flip the z axis in the old convention if frame_convention == 1: diff --git a/ec_grasp_planner/src/planner_utils.py b/ec_grasp_planner/src/planner_utils.py new file mode 100644 index 0000000..6eb3427 --- /dev/null +++ b/ec_grasp_planner/src/planner_utils.py @@ -0,0 +1,51 @@ +import numpy as np +import tf_conversions.posemath as pm +from tf import transformations as tra +from geometry_graph_msgs.msg import geometry_msgs + + +def unit_vector(vector): + # Returns the unit vector of the vector. + return vector / np.linalg.norm(vector) + + +def angle_between(v1, v2): + # Returns the angle in radians between vectors 'v1' and 'v2':: + v1_u = unit_vector(v1) + v2_u = unit_vector(v2) + return np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0)) + + +def convert_pose_msg_to_homogeneous_tf(msg): + return pm.toMatrix(pm.fromMsg(msg)) + + +def convert_homogeneous_tf_to_pose_msg(htf): + return pm.toMsg(pm.fromMatrix(htf)) + + +def convert_transform_msg_to_homogeneous_tf(msg): + return np.dot(tra.translation_matrix([msg.translation.x, msg.translation.y, msg.translation.z]), + tra.quaternion_matrix([msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w])) + + +def tf_dbg_call_to_string(in_transformation, frame_name='dbg'): + msgframe = transform_to_pose_msg(in_transformation) + return "rosrun tf static_transform_publisher {0} {1} {2} {3} {4} {5} {6} rlab_origin {7} 100".format( + msgframe.position.x, msgframe.position.y, msgframe.position.z, msgframe.orientation.x, + msgframe.orientation.y, msgframe.orientation.z, msgframe.orientation.w, frame_name) + + +def transform_to_pose(in_transform): + # convert 4x4 matrix to trans + rot + scale, shear, angles, translation, persp = tra.decompose_matrix(in_transform) + orientation_quat = tra.quaternion_from_euler(angles[0], angles[1], angles[2]) + return translation, orientation_quat + + +def transform_to_pose_msg(in_transform): + trans, rot = transform_to_pose(in_transform) + trans = geometry_msgs.msg.Point(x=trans[0], y=trans[1], z=trans[2]) + rot = geometry_msgs.msg.Quaternion(x=rot[0], y=rot[1], z=rot[2], w=rot[3]) + return geometry_msgs.msg.Pose(position=trans, orientation=rot) + From 47dd269b6305e1bc58a27d92287afdcd5a4f046c Mon Sep 17 00:00:00 2001 From: Johannes Wortmann Date: Mon, 15 Apr 2019 20:13:13 +0200 Subject: [PATCH 7/8] essential fix for feasibilty check (got lost during merges. was delivered during ambassador meeting but wrongly not in master_DisneyMS5 --- ec_grasp_planner/src/planner.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ec_grasp_planner/src/planner.py b/ec_grasp_planner/src/planner.py index e3af220..05538ec 100755 --- a/ec_grasp_planner/src/planner.py +++ b/ec_grasp_planner/src/planner.py @@ -297,7 +297,9 @@ def handle_run_grasp_planner(self, req): ha, self.rviz_frames = hybrid_automaton_from_motion_sequence(grasp_path, graph, graph_in_base, object_in_base, self.handarm_params, self.object_type, frame_convention, self.handover, - self.multi_object_handler.get_object_params()) + self.multi_object_handler.get_object_params(), + self.multi_object_handler.get_alternative_behavior( + chosen_object_idx, chosen_node_idx)) # -------------------------------------------------------- # Output the hybrid automaton From 22590db50197f7cdaf752a983600a40b38846af3 Mon Sep 17 00:00:00 2001 From: Johannes Wortmann Date: Mon, 15 Apr 2019 20:31:28 +0200 Subject: [PATCH 8/8] removed some commented code --- ec_grasp_planner/src/multi_object_params.py | 30 ++------------------- 1 file changed, 2 insertions(+), 28 deletions(-) diff --git a/ec_grasp_planner/src/multi_object_params.py b/ec_grasp_planner/src/multi_object_params.py index 17e5e0c..a82a6f3 100755 --- a/ec_grasp_planner/src/multi_object_params.py +++ b/ec_grasp_planner/src/multi_object_params.py @@ -137,16 +137,13 @@ def pdf_object_ec(self, object, ec_frame, strategy): def black_list_walls(self, current_ec_index, all_ec_frames, strategy): - print(":::A1") if strategy not in ["WallGrasp", "EdgeGrasp"]: return 1 # this function will blacklist all walls except # the one on th right side of the robot # y coord is the smallest - print(":::A2") if all_ec_frames[current_ec_index][1,3] > 0: - print(":::A3") return 0 min_y = 10000 @@ -158,10 +155,8 @@ def black_list_walls(self, current_ec_index, all_ec_frames, strategy): min_y_index = i if min_y_index == current_ec_index: - print(":::A4") return 1 else: - print(":::A5") return 0 def black_list_unreachable_zones(self, object, object_params, ifco_in_base_transform, strategy): @@ -230,8 +225,6 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in edges = [multi_object_params.transform_to_pose_msg(e.transform) for e in all_ecs if e.label == "EdgeGrasp"] # The backup table frame that is used in case we don't create the table from edges - #table_frame = None - #table_pose = geometry_msgs.msg.Pose() table_frame = tra.inverse_matrix(ifco_in_base_transform) # ec_frame (does not have the correct orientation) table_pose = multi_object_params.transform_to_pose_msg(table_frame) @@ -638,31 +631,12 @@ def check_kinematic_feasibility(self, current_object_idx, objects, current_ec_in # initialize stored trajectories for the given object self.stored_trajectories[(current_object_idx, current_ec_index)] = {} - # Try to create the table from edges. As fall back use SurfaceGrasp frame (which might have a wrong orientation) + # Try to create the table from edges. table_from_edges = True if len(edges) < 2: - - # we use the SurfaceGrasp frame as a backup + # we use the backup table frame (constant table dimensions) table_from_edges = False - # if table_frame is None: - # # we did not already compute a table pose backup... - # # Find potential SurfaceGrasp frames that can be used as a backup for table frames - # pot_table_frames = [e.transform for e in all_ecs if e.label == 'SurfaceGrasp'] - - # if len(pot_table_frames) > 0: - # table_frame = pot_table_frames[0] - - # # Since the surface grasp frame is at the object center we have to translate it in z direction - # table_frame[2, 3] = table_frame[2, 3] - object['bounding_box'].z / 2.0 - - # Convert table frame to pose message - # table_pose = multi_object_params.transform_to_pose_msg(table_frame) - - # else: - # # TODO limitation of the feasibility checker which needs at least two edges to create a table - # raise ValueError("Planning for {}, but not enough edges/table frames found".format(strategy)) - if table_frame is not None: print("TABLE POSE", table_pose)