def remove_ordered_collisions(self, ordered_collisions): ''' Removes the ordered collisions from the current diff. This will ONLY remove ordered collisions by removing them from the diff. It can be used to exactly undo the effects of add_ordered_collisions, but cannot be used to change any collisions that were not set using add_ordered_collisions. To enable or disable collisions in the scene, use add_ordered_collisions. For example, if you used add_ordered_collisions to add a collision operation that removed all collisions with the right gripper, passing the same collision operation to this function will reset the collisions to exactly what they were before. It will NOT enable the collisions of the gripper with everything. **Args:** **ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):** ordered collisions to remove ''' if not ordered_collisions or\ not ordered_collisions.collision_operations: return newops = OrderedCollisionOperations() for o in self.current_diff.operations.collision_operations: doadd = True for a in ordered_collisions.collision_operations: if o.object1 == a.object1 and o.object2 == a.object2 and\ o.operation == a.operation: doadd = False break if doadd: newops.collision_operations.append(o) self.set_collisions(newops)
def build_collision_operations(self, object1, object2): msg = OrderedCollisionOperations() collision = CollisionOperation() collision.operation = CollisionOperation.DISABLE collision.object1 = object1 collision.object2 = object2 msg.collision_operations.append(collision) return msg
def get_collisions(self): ''' creates list of collisions to be ignored. @rtype OrderedCollisionOperations @returns list of collisions to be ignored ''' ignore = OrderedCollisionOperations() gripper = CollisionOperation() gripper.object1 = "l_end_effector" gripper.object2 = CollisionOperation.COLLISION_SET_ALL gripper.operation = CollisionOperation.DISABLE wrist = CollisionOperation() wrist.object1 = "l_wrist_roll_link" wrist.object2 = CollisionOperation.COLLISION_SET_ALL wrist.operation = CollisionOperation.DISABLE obj = CollisionOperation() obj.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS obj.object2 = "table" obj.operation = CollisionOperation.DISABLE map_obj = CollisionOperation() map_obj.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS map_obj.object2 = "collision_map" map_obj.operation = CollisionOperation.DISABLE lfing = CollisionOperation() lfing.object1 = "r_gripper_r_finger_tip_link" lfing.object2 = "table" lfing.operation = CollisionOperation.DISABLE rfingtip = CollisionOperation() rfingtip.object1 = "r_gripper_l_finger_tip_link" rfingtip.object2 = "table" rfingtip.operation = CollisionOperation.DISABLE rfing = CollisionOperation() rfing.object1 = "r_gripper_l_finger_link" rfing.object2 = "table" rfing.operation = CollisionOperation.DISABLE forearm = CollisionOperation() forearm.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS forearm.object2 = "l_forearm_link" forearm.operation = CollisionOperation.DISABLE flex = CollisionOperation() flex.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS flex.object2 = "l_wrist_flex_link" flex.operation = CollisionOperation.DISABLE ops = [] op = CollisionOperation() op.operation = op.DISABLE op.object1 = op.COLLISION_SET_ATTACHED_OBJECTS for l in self.my_world.hands['left_arm'].hand_links: op.object2 = l ops.append(copy.deepcopy(op)) ignore.collision_operations = [ gripper, wrist, obj, map_obj, lfing, rfing, rfingtip, forearm, flex ] + ops return ignore
def ready_arm(self, collision_operations=OrderedCollisionOperations()): """ Moves the arm to the side out of the view of all sensors. In this position the arm is ready to perform a grasp action. """ goal = ReadyArmGoal() goal.collision_operations = collision_operations state = self.ready_arm_client.send_goal_and_wait(goal) return state == GoalStatus.SUCCEEDED
def reset_robot(self, tabletop_collision_map_processing_result=None): rospy.loginfo('resetting robot') ordered_collision_operations = OrderedCollisionOperations() if tabletop_collision_map_processing_result: closest_index = self.info[0][0] collision_object_name = tabletop_collision_map_processing_result.collision_object_names[ closest_index] collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name collision_operation = CollisionOperation() collision_operation.object1 = collision_support_surface_name collision_operation.object2 = ARM_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations = [ collision_operation ] collision_operation = CollisionOperation() collision_operation.object1 = collision_support_surface_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations.append( collision_operation) collision_operation = CollisionOperation() collision_operation.object1 = collision_object_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE collision_operation.penetration_distance = 0.02 ordered_collision_operations.collision_operations.append( collision_operation) else: self.reset_collision_map() current_state = find_current_arm_state() rospy.loginfo('arm is currently in %s state' % current_state) if current_state not in ['READY', 'TUCK']: # check if the gripper is empty grasp_status = self.get_grasp_status_srv() if grasp_status.is_hand_occupied: rospy.loginfo( 'arm is not in any of the following states %s, opening gripper' % str(['READY', 'TUCK'])) self.open_gripper() if current_state != 'READY' and not self.ready_arm( ordered_collision_operations): return False self.gentle_close_gripper() return True
def set_collisions(self, ordered_collisions): ''' Sets the ordered collisions in the scene, removing any ordered collisions you passed previously. If you pass in None, this removes all ordered collisions. **Args:** **ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):** ordered collisions ''' if not ordered_collisions: self.current_diff.operations = OrderedCollisionOperations() else: self.current_diff.operations = ordered_collisions self.send_diff() return True
def execute_trajectories(self, result): rospy.loginfo('There are ' + str(len(result.primitive_names)) + ' segments') for (i, t) in enumerate(result.primitive_types): rospy.loginfo('Executing trajectory ' + str(i) + ' of type ' + t + ' and length ' + str( len(result.primitive_trajectories[i]. joint_trajectory.points))) #print 'Trajectory is\n', result.primitive_trajectories[i] splits = t.split('-') if splits[0] == 'BaseTransit': self.execute_base_trajectory(result.primitive_trajectories[i]) continue if splits[0] == 'BaseWarp': self.arm_tasks.move_arm_to_side('right_arm') self.arm_tasks.move_arm_to_side('left_arm') self.execute_warp_trajectory(result.primitive_trajectories[i]) continue if splits[0] == 'ArmTransit': self.execute_arm_trajectory(splits[1], result.primitive_trajectories[i]) continue #if splits[0] == 'Approach': #gripper.open() if splits[0] == 'Pickup': try: self.grippers[splits[1]].close() except ActionFailedError: pass self.wi.attach_object_to_gripper(splits[1], splits[2]) #so the trajectory filter doesn't complain ops = OrderedCollisionOperations() op = CollisionOperation() op.operation = op.DISABLE op.object2 = splits[2] for t in self.wi.hands[splits[1]].touch_links: op.object1 = t ops.collision_operations.append(copy.deepcopy(op)) self.psi.add_ordered_collisions(ops) self.execute_straight_line_arm_trajectory( splits[1], result.primitive_trajectories[i]) if splits[0] == 'Place': self.grippers[splits[1]].open() self.wi.detach_and_add_back_attached_object( splits[1], splits[2]) self.psi.reset()
def move_arm_joint_goal(move_arm_client, joint_names, joint_positions, allowed_contacts=[], link_padding=[], collision_operations=OrderedCollisionOperations()): goal = MoveArmGoal() goal.planner_service_name = 'ompl_planning/plan_kinematic_path' goal.motion_plan_request.planner_id = '' goal.motion_plan_request.group_name = ARM_GROUP_NAME goal.motion_plan_request.num_planning_attempts = 3 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0) goal.motion_plan_request.goal_constraints.joint_constraints = [ JointConstraint(j, p, 0.1, 0.1, 0.0) for (j, p) in zip(joint_names, joint_positions) ] goal.planning_scene_diff.allowed_contacts = allowed_contacts goal.planning_scene_diff.link_padding = link_padding goal.operations = collision_operations move_arm_client.send_goal(goal) finished_within_time = move_arm_client.wait_for_result( rospy.Duration(200.0)) if not finished_within_time: move_arm_client.cancel_goal() rospy.logerr('timed out trying to achieve joint goal') return False else: state = move_arm_client.get_state() if state == GoalStatus.SUCCEEDED: return True else: rospy.logerr( 'failed to achieve joint goal (returned status code %s)' % str(state)) return False
def put_down_at(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() bbox_center = self.find_cluster_bounding_box_center( goal.graspable_object.cluster) if not bbox_center: self.action_server.set_aborted() goal.target_location.z = bbox_center.z x_dist = goal.target_location.x - bbox_center.x y_dist = goal.target_location.y - bbox_center.y target = goal.graspable_object target.cluster.points = [ Point32(p.x + x_dist, p.y + y_dist, p.z) for p in target.cluster.points ] collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name) if ik_solution: # disable collisions between gripper and target object ordered_collision_operations = OrderedCollisionOperations() collision_operation = CollisionOperation() collision_operation.object1 = collision_object_name collision_operation.object2 = GRIPPER_GROUP_NAME collision_operation.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.01 # disable collisions between arm and table collision_operation3 = CollisionOperation() collision_operation3.object1 = collision_support_surface_name collision_operation3.object2 = ARM_GROUP_NAME collision_operation3.operation = CollisionOperation.DISABLE collision_operation3.penetration_distance = 0.01 ordered_collision_operations.collision_operations = [ collision_operation, collision_operation2, collision_operation3 ] # set gripper padding to 0 gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] gripper_paddings.extend([LinkPadding(l, 0.0) for l in ARM_LINKS]) # move into pre-grasp pose if not move_arm_joint_goal( self.move_arm_client, ik_solution.joint_state.name, ik_solution.joint_state.position, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): rospy.logerr('%s: attempted grasp failed' % ACTION_NAME) self.action_server.set_aborted() return # record put down sound with 0.5 second padding before and after self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id) rospy.sleep(0.5) self.open_gripper() rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) self.action_server.set_succeeded( PutDownAtResult(sound_msg.recorded_sound)) return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted put down failed' % ACTION_NAME) self.action_server.set_aborted()
def grasp_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() target = goal.graspable_object collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name) if ik_solution: # disable collisions between gripper and target object ordered_collision_operations = OrderedCollisionOperations() collision_operation1 = CollisionOperation() collision_operation1.object1 = collision_object_name collision_operation1.object2 = GRIPPER_GROUP_NAME collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE # collision_operation2.penetration_distance = 0.02 ordered_collision_operations.collision_operations = [ collision_operation1, collision_operation2 ] # set gripper padding to 0 gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] gripper_paddings.extend([LinkPadding(l, 0.0) for l in ARM_LINKS]) self.open_gripper() # move into pre-grasp pose if not move_arm_joint_goal( self.move_arm_client, ik_solution.joint_state.name, ik_solution.joint_state.position, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): rospy.logerr('%s: attempted grasp failed' % ACTION_NAME) self.action_server.set_aborted() return # record grasping sound with 0.5 second padding before and after self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id) rospy.sleep(0.5) grasp_successful = self.close_gripper() rospy.sleep(0.5) # if grasp was successful, attach it to the gripper if grasp_successful: sound_msg = self.stop_audio_recording_srv(True) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) self.action_server.set_succeeded( GraspObjectResult(sound_msg.recorded_sound)) return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted grasp failed' % ACTION_NAME) self.action_server.set_aborted()
def run(self): #(goal, mass, prims, obs): search_start = time.time() obj_name = sys.argv[1] table_height = self.get_table_height() self.goal.pose.position.z = table_height + 0.01 print "goal=" + str(self.goal) self.make_marker(self.goal, "goal_pose", mtype=Marker.ARROW, color=[0.0, 1.0, 0.0, 0.8]) obj = self.get_obj(obj_name) shape = self.get_shape(obj) obs = self.make_gripper_obs() mobj = Mobject(mass, shape, prims) pose = PoseStamped() pose.header.frame_id = "/torso_lift_link" pose.pose.position.x = goal.pose.position.x pose.pose.position.y = goal.pose.position.y pose.pose.position.z = goal.pose.position.z + 0.02 #.1 pose.pose.orientation = goal.pose.orientation vels = [0.0, 0.0, 0.0] # self.place('right_arm', obj_name, pose) print "DIMS=" + str(shape.dimensions) (release_pose, wrist_release) = self.get_best_release(mobj, obj, goal) # release_pose = pose self.make_marker(release_pose, namespace="release_pose") print "release_pose=" + str(release_pose) search_end = time.time() # block_start = time.time() # dof = mobj.find_dof(goal, release_pose, vels) # print "DOF="+str(dof.degrees) # success = False # order = dof.index_order() # while not success: # not passive placing # if len(order) != 0.0: # next = order.pop(0) # print "NEXT="+str(next) # success = self.block(obs, next, dof[next], shape, goal) # print "SUCCESS="+str(success) # else: # print "BLOCK FAILED" # break # block_end = time.time() print "waiting for place:" # raw_input() ignore = OrderedCollisionOperations() gripper = CollisionOperation() gripper.object1 = CollisionOperation.COLLISION_SET_ALL #"l_end_effector" gripper.object2 = CollisionOperation.COLLISION_SET_ALL gripper.operation = CollisionOperation.DISABLE wrist = CollisionOperation() wrist.object1 = "l_wrist_roll_link" wrist.object2 = CollisionOperation.COLLISION_SET_ALL wrist.operation = CollisionOperation.DISABLE obj = CollisionOperation() obj.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS obj.object2 = "table" #CollisionOperation.COLLISION_SET_ALL obj.operation = CollisionOperation.DISABLE ignore.collision_operations = [gripper, wrist, obj] self.move("right_arm", wrist_release, ignore) # self.release("right_arm") # self.place('right_arm', obj_name, release_pose) search = search_end - search_start block = 0.0 #block_end - block_start return (search, block)
graspquat, start_angles, pos_spacing=0.02, collision_aware=1, collision_check_resolution=1, steps_before_abort=-1) #print "using current start angles" #check_cartesian_path_lists(approachpos, approachquat, grasppos, graspquat, start_angles = [], pos_spacing = 0.02, collision_aware = 1, collision_check_resolution = 1, steps_before_abort = -1) print "ignoring collisions with all collision points" collision_oper = CollisionOperation(object1 = "points", \ object2 = CollisionOperation.COLLISION_SET_ALL, \ operation = CollisionOperation.DISABLE) ordered_collision_operations = OrderedCollisionOperations([ collision_oper, ]) check_cartesian_path_lists( approachpos, approachquat, grasppos, graspquat, start_angles, pos_spacing=0.02, collision_aware=1, collision_check_resolution=1, steps_before_abort=-1, ordered_collision_operations=ordered_collision_operations) print "abort early" check_cartesian_path_lists(approachpos,
def move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts=[], link_padding=[], collision_operations=OrderedCollisionOperations()): move_arm_client = SimpleActionClient('move_left_arm', MoveArmAction) move_arm_client.wait_for_server() rospy.loginfo('connected to move_left_arm action server') goal = MoveArmGoal() goal.planner_service_name = 'ompl_planning/plan_kinematic_path' goal.motion_plan_request.planner_id = '' goal.motion_plan_request.group_name = 'left_arm' goal.motion_plan_request.num_planning_attempts = 1 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0) goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint(j, p, 0.1, 0.1, 0.0) for (j,p) in zip(joint_names,joint_positions)] goal.motion_plan_request.allowed_contacts = allowed_contacts goal.motion_plan_request.link_padding = link_padding goal.motion_plan_request.ordered_collision_operations = collision_operations move_arm_client.send_goal(goal) finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0)) if not finished_within_time: move_arm_client.cancel_goal() rospy.loginfo('timed out trying to achieve a joint goal') else: state = move_arm_client.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo('action finished: %s' % str(state)) return True else: rospy.loginfo('action failed: %s' % str(state)) return False
def main(): print 'Starting!' parser = _get_options_parser() (options, args) = parser.parse_args() print 'pause =', options.pause ss = SimSetup(options.world, False) world = ss.setup() planner = Planner() base_poses = get_base_poses(ss.wi, ss.world) arm_poses = get_arm_poses(ss.wi, ss.world) time = 0 #note: these should all update the planning scene as we go #first plan to initial base position stime = planner.plan_base(base_poses.push) rospy.loginfo("Initial base move: " + str(stime)) time += stime print 'pause =', options.pause if options.pause: raw_input() #now plan to approach to push stime = planner.plan_arm(arm_poses.approach_push) rospy.loginfo('Gross move to push: ' + str(stime)) time += stime if options.pause: raw_input() ops = OrderedCollisionOperations() op = CollisionOperation() op.operation = op.DISABLE op.object1 = world.object.id op.object2 = world.object_support_surface ops.collision_operations.append(op) op = copy.deepcopy(op) op.object2 = 'r_end_effector' ops.collision_operations.append(op) op = copy.deepcopy(op) op.object1 = world.object_support_surface ops.collision_operations.append(op) stime = planner.plan_arm_interpolated(arm_poses.init_push, ordered_colls=ops) rospy.loginfo('Approach to push: ' + str(stime)) time += stime if options.pause: raw_input() stime = planner.plan_arm_interpolated(arm_poses.final_push, ordered_colls=ops) rospy.loginfo('Push: ' + str(stime)) time += stime co = copy.deepcopy(world.object) co.poses[0].position.x -= PUSH_DIST ss.wi.add_object(co) planner.psi.reset() if options.pause: raw_input() stime = planner.plan_arm_interpolated(arm_poses.retreat_push, ordered_colls=ops) rospy.loginfo('Retreat: ' + str(stime)) time += stime if options.pause: raw_input() stime = planner.plan_base(base_poses.pick) rospy.loginfo('Base move to pick: ' + str(stime)) time += stime if options.pause: raw_input() stime = planner.plan_arm(arm_poses.approach_pick) rospy.loginfo('Gross move to Pick: ' + str(stime)) time += stime if options.pause: raw_input() stime = planner.plan_arm_interpolated(arm_poses.pick, ordered_colls=ops) rospy.loginfo('Approach to pick: ' + str(stime)) time += stime if options.pause: raw_input() stime = planner.plan_arm_interpolated(arm_poses.lift, ordered_colls=ops) rospy.loginfo('Lift: ' + str(stime)) time += stime if options.pause: raw_input() for g in base_poses.goal: stime = planner.plan_base(g) rospy.loginfo('Base move to goal: ' + str(stime)) time += stime if options.pause: raw_input() stime = planner.plan_arm(arm_poses.goal) rospy.loginfo('Arm move to goal: ' + str(stime)) time += stime if options.pause: raw_input() rospy.loginfo('TIME = ' + str(time))
def push_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name current_state = rospy.wait_for_message('l_arm_controller/state', FollowJointTrajectoryFeedback) start_angles = current_state.actual.positions full_state = rospy.wait_for_message('/joint_states', JointState) req = GetPositionFKRequest() req.header.frame_id = 'base_link' req.fk_link_names = [GRIPPER_LINK_FRAME] req.robot_state.joint_state = full_state if not self.set_planning_scene_diff_client(): rospy.logerr('%s: failed to set planning scene diff' % ACTION_NAME) self.action_server.set_aborted() return pose = self.get_fk_srv(req).pose_stamped[0].pose frame_id = 'base_link' approachpos = [pose.position.x, pose.position.y, pose.position.z] approachquat = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] push_distance = 0.40 grasppos = [ pose.position.x, pose.position.y - push_distance, pose.position.z ] graspquat = [0.00000, 0.00000, 0.70711, -0.70711] # attach object to gripper, they will move together obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.02 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [ collision_operation1, collision_operation2 ] res = self.check_cartesian_path_lists( approachpos, approachquat, grasppos, graspquat, start_angles, frame=frame_id, ordered_collision_operations=ordered_collision_operations) error_code_dict = { ArmNavigationErrorCodes.SUCCESS: 0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1, ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2, ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3, ArmNavigationErrorCodes.PLANNING_FAILED: 4 } trajectory_len = len(res.trajectory.joint_trajectory.points) trajectory = [ res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len) ] vels = [ res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len) ] times = [ res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len) ] error_codes = [ error_code_dict[error_code.val] for error_code in res.trajectory_error_codes ] # if even one code is not 0, abort if sum(error_codes) != 0: for ind in range(len(trajectory)): rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind])) rospy.loginfo("") for ind in range(len(trajectory)): rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind])) rospy.logerr('%s: attempted push failed' % ACTION_NAME) self.action_server.set_aborted() return req = FilterJointTrajectoryRequest() req.trajectory = res.trajectory.joint_trajectory req.trajectory.points = req.trajectory.points[ 1:] # skip zero velocity point req.allowed_time = rospy.Duration(2.0) filt_res = self.trajectory_filter_srv(req) goal = FollowJointTrajectoryGoal() goal.trajectory = filt_res.trajectory goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id) rospy.sleep(0.5) self.trajectory_controller.send_goal(goal) self.trajectory_controller.wait_for_result() state = self.trajectory_controller.get_state() if state == GoalStatus.SUCCEEDED: rospy.sleep(0.5) sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded( PushObjectResult(sound_msg.recorded_sound)) return rospy.logerr('%s: attempted push failed' % ACTION_NAME) self.stop_audio_recording_srv(False) self.action_server.set_aborted()
def place_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_object_name = goal.collision_object_name collision_support_surface_name = goal.collision_support_surface_name # check that we have something in hand before placing it grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if not grasp_status.is_hand_occupied: rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME) self.action_server.set_aborted() return gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] # disable collisions between attached object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = collision_support_surface_name collision_operation2.object2 = GRIPPER_GROUP_NAME collision_operation2.operation = CollisionOperation.DISABLE collision_operation2.penetration_distance = 0.01 # disable collisions between arm and table collision_operation3 = CollisionOperation() collision_operation3.object1 = collision_support_surface_name collision_operation3.object2 = ARM_GROUP_NAME collision_operation3.operation = CollisionOperation.DISABLE collision_operation3.penetration_distance = 0.01 ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [ collision_operation1, collision_operation2, collision_operation3 ] self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id) if move_arm_joint_goal( self.move_arm_client, ARM_JOINTS, PLACE_POSITION, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): sound_msg = None grasp_status = self.get_grasp_status_srv() self.open_gripper() rospy.sleep(0.5) # if after lowering arm gripper is still holding an object life's good if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) else: self.stop_audio_recording_srv(False) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT obj.object.id = collision_object_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) if sound_msg: self.action_server.set_succeeded( PlaceObjectResult(sound_msg.recorded_sound)) return else: self.action_server.set_aborted() return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted place failed' % ACTION_NAME) self.action_server.set_aborted()
def lift_object(self, goal): if self.action_server.is_preempt_requested(): rospy.loginfo('%s: preempted' % ACTION_NAME) self.action_server.set_preempted() collision_support_surface_name = goal.collision_support_surface_name # disable collisions between grasped object and table collision_operation1 = CollisionOperation() collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation1.object2 = collision_support_surface_name collision_operation1.operation = CollisionOperation.DISABLE # disable collisions between gripper and table collision_operation2 = CollisionOperation() collision_operation2.object1 = GRIPPER_GROUP_NAME collision_operation2.object2 = collision_support_surface_name collision_operation2.operation = CollisionOperation.DISABLE ordered_collision_operations = OrderedCollisionOperations() ordered_collision_operations.collision_operations = [ collision_operation1, collision_operation2 ] gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS] # this is a hack to make arm lift an object faster obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = GRIPPER_LINK_FRAME obj.object.operation.operation = CollisionObjectOperation.REMOVE obj.object.id = collision_support_surface_name obj.link_name = GRIPPER_LINK_FRAME obj.touch_links = GRIPPER_LINKS self.attached_object_pub.publish(obj) current_state = rospy.wait_for_message('l_arm_controller/state', FollowJointTrajectoryFeedback) joint_names = current_state.joint_names joint_positions = current_state.actual.positions start_angles = [ joint_positions[joint_names.index(jn)] for jn in ARM_JOINTS ] start_angles[0] = start_angles[0] - 0.3 # move shoulder up a bit if not move_arm_joint_goal( self.move_arm_client, ARM_JOINTS, start_angles, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): self.action_server.set_aborted() return self.start_audio_recording_srv(InfomaxAction.LIFT, goal.category_id) if move_arm_joint_goal( self.move_arm_client, ARM_JOINTS, LIFT_POSITION, link_padding=gripper_paddings, collision_operations=ordered_collision_operations): # check if are still holding an object after lift is done grasp_status = self.get_grasp_status_srv() # if the object is still in hand after lift is done we are good if grasp_status.is_hand_occupied: sound_msg = self.stop_audio_recording_srv(True) self.action_server.set_succeeded( LiftObjectResult(sound_msg.recorded_sound)) return self.stop_audio_recording_srv(False) rospy.logerr('%s: attempted lift failed' % ACTION_NAME) self.action_server.set_aborted() return