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 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 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 build_collision_operations(self, arm_name, penetration_depth, enable=False): msg = OrderedCollisionOperations() if arm_name == "right_arm": link_names = ["r_gripper_palm_joint", "r_gripper_l_finger_joint", "r_gripper_l_finger_tip_joint", "r_gripper_led_joint", "r_gripper_motor_accelerometer_joint", "r_gripper_motor_slider_joint", "r_gripper_motor_screw_joint", "r_gripper_r_finger_joint", "r_gripper_r_finger_tip_joint", "r_gripper_joint", "r_gripper_tool_joint", ] else: link_names = ["l_gripper_palm_joint", "l_gripper_l_finger_joint", "l_gripper_l_finger_tip_joint", "l_gripper_led_joint", "l_gripper_motor_accelerometer_joint", "l_gripper_motor_slider_joint", "l_gripper_motor_screw_joint", "l_gripper_r_finger_joint", "l_gripper_r_finger_tip_joint", "l_gripper_joint", "l_gripper_tool_joint", ] for link in link_names: collision = CollisionOperation() if enable: collision.operation = CollisionOperation.ENABLE else: collision.operation = CollisionOperation.DISABLE collision.object1 = link collision.object2 = "collision_map" collision.penetration_distance = penetration_depth 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 test_attached_object_collision_operations(self): global set_allowed_collision_service_name global get_allowed_collision_service_name global revert_allowed_collision_service_name full_name = default_prefix + '/' + set_allowed_collision_service_name rospy.wait_for_service(full_name) set_allowed_collision_service_proxy = rospy.ServiceProxy( full_name, arm_navigation_msgs.srv.SetAllowedCollisions) full_name = default_prefix + '/' + get_allowed_collision_service_name rospy.wait_for_service(full_name) get_allowed_collision_service_proxy = rospy.ServiceProxy( full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix) full_name = default_prefix + '/' + revert_allowed_collision_service_name rospy.wait_for_service(full_name) revert_allowed_collision_service_proxy = rospy.ServiceProxy( full_name, std_srvs.srv.Empty) att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject) rospy.init_node('test_allowed_collisions') att_object = AttachedCollisionObject() att_object.object.header.stamp = rospy.Time.now() att_object.object.header.frame_id = "base_link" att_object.link_name = "r_gripper_r_finger_tip_link" att_object.touch_links = [str() for _ in range(1)] att_object.touch_links[0] = "r_gripper_palm_link" att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD att_object.object = CollisionObject() att_object.object.header.stamp = rospy.Time.now() att_object.object.header.frame_id = "base_link" att_object.object.id = "obj3" att_object.object.shapes = [Shape() for _ in range(1)] att_object.object.shapes[0].type = Shape.BOX att_object.object.shapes[0].dimensions = [float() for _ in range(3)] att_object.object.shapes[0].dimensions[0] = .2 att_object.object.shapes[0].dimensions[1] = 0.3 att_object.object.shapes[0].dimensions[2] = .05 att_object.object.poses = [Pose() for _ in range(1)] att_object.object.poses[0].position.x = .15 att_object.object.poses[0].position.y = 0 att_object.object.poses[0].position.z = 1.0 att_object.object.poses[0].orientation.x = 0 att_object.object.poses[0].orientation.y = 0 att_object.object.poses[0].orientation.z = 0 att_object.object.poses[0].orientation.w = 1 att_pub.publish(att_object) rospy.sleep(5.0) get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest() get_res = get_allowed_collision_service_proxy(get_mat) try: r_gripper_palm_link_index = get_res.matrix.link_names.index( "r_gripper_palm_link") except ValueError: self.fail( "No r_gripper_palm_link in AllowedCollisionMatrix link names") try: r_gripper_r_finger_tip_link_index = get_res.matrix.link_names.index( "r_gripper_r_finger_tip_link") except ValueError: self.fail( "No r_gripper_r_finger_tip_link in AllowedCollisionMatrix link names" ) try: obj3_index = get_res.matrix.link_names.index("obj3") except ValueError: self.fail("No obj3 in AllowedCollisionMatrix link names") self.assertEqual( True, get_res.matrix.entries[obj3_index]. enabled[r_gripper_r_finger_tip_link_index]) self.assertEqual( True, get_res.matrix.entries[r_gripper_r_finger_tip_link_index]. enabled[obj3_index]) self.assertEqual( True, get_res.matrix.entries[obj3_index]. enabled[r_gripper_palm_link_index]) self.assertEqual( True, get_res.matrix.entries[r_gripper_palm_link_index]. enabled[obj3_index]) #this should overwrite touch links set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest( ) set_allowed_request.ord.collision_operations = [ CollisionOperation() for _ in range(1) ] set_allowed_request.ord.collision_operations[0].object1 = "obj3" set_allowed_request.ord.collision_operations[ 0].object2 = "r_gripper_palm_link" set_allowed_request.ord.collision_operations[ 0].operation = CollisionOperation.ENABLE set_allowed_collision_service_proxy(set_allowed_request) rospy.sleep(1.) get_res = get_allowed_collision_service_proxy(get_mat) self.assertEqual( False, get_res.matrix.entries[obj3_index]. enabled[r_gripper_palm_link_index]) self.assertEqual( False, get_res.matrix.entries[r_gripper_palm_link_index]. enabled[obj3_index]) set_allowed_request.ord.collision_operations[ 0].object1 = "r_gripper_palm_link" set_allowed_request.ord.collision_operations[ 0].object2 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS set_allowed_request.ord.collision_operations[ 0].operation = CollisionOperation.DISABLE set_allowed_collision_service_proxy(set_allowed_request) rospy.sleep(1.) get_res = get_allowed_collision_service_proxy(get_mat) self.assertEqual( True, get_res.matrix.entries[obj3_index]. enabled[r_gripper_palm_link_index]) self.assertEqual( True, get_res.matrix.entries[r_gripper_palm_link_index]. enabled[obj3_index])
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 testAllowedNotAllowedInitialContact(self): #adding object in collision with base obj2 = CollisionObject() obj2.header.stamp = rospy.Time.now() obj2.header.frame_id = "base_link" obj2.id = "base_object" obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD obj2.shapes = [Shape() for _ in range(1)] obj2.shapes[0].type = Shape.BOX obj2.shapes[0].dimensions = [float() for _ in range(3)] obj2.shapes[0].dimensions[0] = .1 obj2.shapes[0].dimensions[1] = .1 obj2.shapes[0].dimensions[2] = .1 obj2.poses = [Pose() for _ in range(1)] obj2.poses[0].position.x = 0 obj2.poses[0].position.y = 0 obj2.poses[0].position.z = 0 obj2.poses[0].orientation.x = 0 obj2.poses[0].orientation.y = 0 obj2.poses[0].orientation.z = 0 obj2.poses[0].orientation.w = 1 self.obj_pub.publish(obj2) rospy.sleep(5.) joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']] goal = MoveArmGoal() goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))] goal.motion_plan_request.group_name = "right_arm" goal.motion_plan_request.num_planning_attempts = 1 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.) goal.motion_plan_request.planner_id = "" goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))] for i in range(len(joint_names)): goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i] goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0 goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08 goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08 goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0 goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2 goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2 self.move_arm_action_client.send_goal(goal) r = rospy.Rate(10) while True: cur_state = self.move_arm_action_client.get_state() if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and cur_state != actionlib_msgs.msg.GoalStatus.PENDING): break #should still have succeedeed final_state = self.move_arm_action_client.get_state() self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED) # but we can still overwrite coll = CollisionOperation() coll.object1 = "base_link" coll.object2 = coll.COLLISION_SET_OBJECTS coll.operation = coll.ENABLE goal.motion_plan_request.ordered_collision_operations.collision_operations.append(coll) goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0 goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2 goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2 self.move_arm_action_client.send_goal(goal) r = rospy.Rate(10) while True: cur_state = self.move_arm_action_client.get_state() if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and cur_state != actionlib_msgs.msg.GoalStatus.PENDING): break #should still have succeedeed final_state = self.move_arm_action_client.get_state() self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def test_object_collisions(self): global set_allowed_collision_service_name global get_allowed_collision_service_name global revert_allowed_collision_service_name full_name = default_prefix + '/' + set_allowed_collision_service_name rospy.wait_for_service(full_name) set_allowed_collision_service_proxy = rospy.ServiceProxy( full_name, arm_navigation_msgs.srv.SetAllowedCollisions) full_name = default_prefix + '/' + get_allowed_collision_service_name rospy.wait_for_service(full_name) get_allowed_collision_service_proxy = rospy.ServiceProxy( full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix) full_name = default_prefix + '/' + revert_allowed_collision_service_name rospy.wait_for_service(full_name) revert_allowed_collision_service_proxy = rospy.ServiceProxy( full_name, std_srvs.srv.Empty) obj_pub = rospy.Publisher('collision_object', CollisionObject) att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject) rospy.init_node('test_allowed_collisions') obj1 = CollisionObject() obj1.header.stamp = rospy.Time.now() obj1.header.frame_id = "base_link" obj1.id = "obj1" obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD obj1.shapes = [Shape() for _ in range(2)] obj1.shapes[0].type = Shape.BOX obj1.shapes[0].dimensions = [float() for _ in range(3)] obj1.shapes[0].dimensions[0] = 1.0 obj1.shapes[0].dimensions[1] = 1.0 obj1.shapes[0].dimensions[2] = .05 obj1.shapes[1].type = Shape.BOX obj1.shapes[1].dimensions = [float() for _ in range(3)] obj1.shapes[1].dimensions[0] = 1.0 obj1.shapes[1].dimensions[1] = 1.0 obj1.shapes[1].dimensions[2] = .05 obj1.poses = [Pose() for _ in range(2)] obj1.poses[0].position.x = 1.0 obj1.poses[0].position.y = 0 obj1.poses[0].position.z = .5 obj1.poses[0].orientation.x = 0 obj1.poses[0].orientation.y = 0 obj1.poses[0].orientation.z = 0 obj1.poses[0].orientation.w = 1 obj1.poses[1].position.x = 1.0 obj1.poses[1].position.y = 0 obj1.poses[1].position.z = .75 obj1.poses[1].orientation.x = 0 obj1.poses[1].orientation.y = 0 obj1.poses[1].orientation.z = 0 obj1.poses[1].orientation.w = 1 obj_pub.publish(obj1) obj2 = CollisionObject() obj2.header.stamp = rospy.Time.now() obj2.header.frame_id = "base_link" obj2.id = "obj2" obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD obj2.shapes = [Shape() for _ in range(1)] obj2.shapes[0].type = Shape.BOX obj2.shapes[0].dimensions = [float() for _ in range(3)] obj2.shapes[0].dimensions[0] = 1.0 obj2.shapes[0].dimensions[1] = 1.0 obj2.shapes[0].dimensions[2] = .05 obj2.poses = [Pose() for _ in range(1)] obj2.poses[0].position.x = .15 obj2.poses[0].position.y = 0 obj2.poses[0].position.z = .5 obj2.poses[0].orientation.x = 0 obj2.poses[0].orientation.y = 0 obj2.poses[0].orientation.z = 0 obj2.poses[0].orientation.w = 1 obj_pub.publish(obj2) rospy.sleep(2.0) get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest() get_res = get_allowed_collision_service_proxy(get_mat) try: r_gripper_palm_link_index = get_res.matrix.link_names.index( "r_gripper_palm_link") except ValueError: self.fail( "No r_gripper_palm_link in AllowedCollisionMatrix link names") try: obj1_index = get_res.matrix.link_names.index("obj1") except ValueError: self.fail("No obj1 in AllowedCollisionMatrix link names") self.failIf(len(get_res.matrix.entries) <= r_gripper_palm_link_index) self.failIf(len(get_res.matrix.entries) <= obj1_index) self.failIf( len(get_res.matrix.entries[obj1_index].enabled) <= r_gripper_palm_link_index) self.failIf( len(get_res.matrix.entries[r_gripper_palm_link_index].enabled) <= obj1_index) self.assertEqual( False, get_res.matrix.entries[obj1_index]. enabled[r_gripper_palm_link_index]) self.assertEqual( False, get_res.matrix.entries[r_gripper_palm_link_index]. enabled[obj1_index]) #now disable between all objects #if enable just this link, it should be true set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest( ) set_allowed_request.ord.collision_operations = [ CollisionOperation() for _ in range(1) ] set_allowed_request.ord.collision_operations[ 0].object1 = "r_gripper_palm_link" set_allowed_request.ord.collision_operations[ 0].object2 = CollisionOperation.COLLISION_SET_OBJECTS set_allowed_request.ord.collision_operations[ 0].operation = CollisionOperation.DISABLE set_allowed_collision_service_proxy(set_allowed_request) rospy.sleep(1.) get_res = get_allowed_collision_service_proxy(get_mat) self.assertEqual( True, get_res.matrix.entries[obj1_index]. enabled[r_gripper_palm_link_index]) self.assertEqual( True, get_res.matrix.entries[r_gripper_palm_link_index]. enabled[obj1_index])
def make_disable_allowed_collisions_with_exclusions(all, exclude): ret = [] for i in all: if i not in exclude: coll = CollisionOperation() coll.object1 = i coll.object2 = coll.COLLISION_SET_OBJECTS coll.operation = coll.DISABLE ret.append(coll) coll2 = CollisionOperation() coll2.object1 = i coll2.object2 = coll.COLLISION_SET_ATTACHED_OBJECTS coll2.operation = coll.DISABLE ret.append(coll2) for j in all: if j != i and j not in exclude: coll3 = CollisionOperation() coll3.object1 = i coll3.object2 = j coll3.operation = coll.DISABLE ret.append(coll3) return ret
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 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 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)
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 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 test_self_collisions(self): global set_allowed_collision_service_name global get_allowed_collision_service_name global revert_allowed_collision_service_name full_name = default_prefix + '/' + set_allowed_collision_service_name rospy.wait_for_service(full_name) set_allowed_collision_service_proxy = rospy.ServiceProxy( full_name, arm_navigation_msgs.srv.SetAllowedCollisions) full_name = default_prefix + '/' + get_allowed_collision_service_name rospy.wait_for_service(full_name) get_allowed_collision_service_proxy = rospy.ServiceProxy( full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix) full_name = default_prefix + '/' + revert_allowed_collision_service_name rospy.wait_for_service(full_name) revert_allowed_collision_service_proxy = rospy.ServiceProxy( full_name, std_srvs.srv.Empty) #first we get the default get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest() get_res = get_allowed_collision_service_proxy(get_mat) #entry for left gripper and right gripper should be false - just an example try: r_gripper_palm_link_index = get_res.matrix.link_names.index( "r_gripper_palm_link") except ValueError: self.fail( "No r_gripper_palm_link in AllowedCollisionMatrix link names") try: l_gripper_palm_link_index = get_res.matrix.link_names.index( "l_gripper_palm_link") except ValueError: self.fail( "No l_gripper_palm_link in AllowedCollisionMatrix link names") self.failIf(len(get_res.matrix.entries) <= r_gripper_palm_link_index) self.failIf(len(get_res.matrix.entries) <= l_gripper_palm_link_index) self.failIf( len(get_res.matrix.entries[l_gripper_palm_link_index].enabled) <= r_gripper_palm_link_index) self.failIf( len(get_res.matrix.entries[r_gripper_palm_link_index].enabled) <= l_gripper_palm_link_index) self.assertEqual( False, get_res.matrix.entries[l_gripper_palm_link_index]. enabled[r_gripper_palm_link_index]) self.assertEqual( False, get_res.matrix.entries[r_gripper_palm_link_index]. enabled[l_gripper_palm_link_index]) #if enable just this link, it should be true set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest( ) set_allowed_request.ord.collision_operations = [ CollisionOperation() for _ in range(1) ] set_allowed_request.ord.collision_operations[ 0].object1 = "r_gripper_palm_link" set_allowed_request.ord.collision_operations[ 0].object2 = "l_gripper_palm_link" set_allowed_request.ord.collision_operations[ 0].operation = CollisionOperation.DISABLE set_allowed_collision_service_proxy(set_allowed_request) rospy.sleep(1.0) get_res = get_allowed_collision_service_proxy(get_mat) self.assertEqual( True, get_res.matrix.entries[l_gripper_palm_link_index]. enabled[r_gripper_palm_link_index]) self.assertEqual( True, get_res.matrix.entries[r_gripper_palm_link_index]. enabled[l_gripper_palm_link_index]) #now we enable r_end_effector versus l_end_effector, and it should be false again set_allowed_request.ord.collision_operations[ 0].object1 = "r_end_effector" set_allowed_request.ord.collision_operations[ 0].object2 = "l_end_effector" set_allowed_request.ord.collision_operations[ 0].operation = CollisionOperation.ENABLE set_allowed_collision_service_proxy(set_allowed_request) rospy.sleep(1.0) get_res = get_allowed_collision_service_proxy(get_mat) self.assertEqual( False, get_res.matrix.entries[l_gripper_palm_link_index]. enabled[r_gripper_palm_link_index]) self.assertEqual( False, get_res.matrix.entries[r_gripper_palm_link_index]. enabled[l_gripper_palm_link_index])
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 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
rospy.sleep(1) obj = AttachedCollisionObject() obj.object.header.stamp = rospy.Time.now() obj.object.header.frame_id = 'L7_wrist_roll_link'#'base_link' obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT obj.object.id = coll_map_res.collision_object_names[0] obj.link_name = 'L7_wrist_roll_link' obj.touch_links = ['L7_wrist_roll_link', 'L8_left_finger_link', 'L9_right_finger_link'] attached_object_pub.publish(obj) rospy.sleep(1) ordered_collision_operations = OrderedCollisionOperations() collision_operation = CollisionOperation() collision_operation.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS collision_operation.object2 = coll_map_res.collision_support_surface_name collision_operation.operation = CollisionOperation.DISABLE ordered_collision_operations.collision_operations = [collision_operation] if not move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts, gripper_paddings, ordered_collision_operations): exit(1) rospy.loginfo('Pickup stage has successfully finished. Will place the object now') ############################################################################ ####################### PLACE STAGE START HERE ############################# listener = TransformListener()
def make_disable_allowed_collisions_with_exclusions(all, exclude): ret = [] for i in all: if i not in exclude: coll = CollisionOperation() coll.object1 = i coll.object2 = coll.COLLISION_SET_OBJECTS coll.operation = coll.DISABLE ret.append(coll) coll2 = CollisionOperation() coll2.object1 = i coll2.object2 = coll.COLLISION_SET_ATTACHED_OBJECTS coll2.operation = coll.DISABLE ret.append(coll2) for j in all: if j != i and j not in exclude: coll3 = CollisionOperation() coll3.object1 = i coll3.object2 = j coll3.operation = coll.DISABLE ret.append(coll3) return ret
def testAllowedNotAllowedInitialContact(self): #adding object in collision with base obj2 = CollisionObject() obj2.header.stamp = rospy.Time.now() obj2.header.frame_id = "base_link" obj2.id = "base_object" obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD obj2.shapes = [Shape() for _ in range(1)] obj2.shapes[0].type = Shape.BOX obj2.shapes[0].dimensions = [float() for _ in range(3)] obj2.shapes[0].dimensions[0] = .1 obj2.shapes[0].dimensions[1] = .1 obj2.shapes[0].dimensions[2] = .1 obj2.poses = [Pose() for _ in range(1)] obj2.poses[0].position.x = 0 obj2.poses[0].position.y = 0 obj2.poses[0].position.z = 0 obj2.poses[0].orientation.x = 0 obj2.poses[0].orientation.y = 0 obj2.poses[0].orientation.z = 0 obj2.poses[0].orientation.w = 1 self.obj_pub.publish(obj2) rospy.sleep(5.) joint_names = [ '%s_%s' % ('r', j) for j in [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ] ] goal = MoveArmGoal() goal.motion_plan_request.goal_constraints.joint_constraints = [ JointConstraint() for i in range(len(joint_names)) ] goal.motion_plan_request.group_name = "right_arm" goal.motion_plan_request.num_planning_attempts = 1 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.) goal.motion_plan_request.planner_id = "" goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request.goal_constraints.joint_constraints = [ JointConstraint() for i in range(len(joint_names)) ] for i in range(len(joint_names)): goal.motion_plan_request.goal_constraints.joint_constraints[ i].joint_name = joint_names[i] goal.motion_plan_request.goal_constraints.joint_constraints[ i].position = 0.0 goal.motion_plan_request.goal_constraints.joint_constraints[ i].tolerance_above = 0.08 goal.motion_plan_request.goal_constraints.joint_constraints[ i].tolerance_below = 0.08 goal.motion_plan_request.goal_constraints.joint_constraints[ 0].position = -2.0 goal.motion_plan_request.goal_constraints.joint_constraints[ 3].position = -0.2 goal.motion_plan_request.goal_constraints.joint_constraints[ 5].position = -0.2 self.move_arm_action_client.send_goal(goal) r = rospy.Rate(10) while True: cur_state = self.move_arm_action_client.get_state() if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and cur_state != actionlib_msgs.msg.GoalStatus.PENDING): break #should still have succeedeed final_state = self.move_arm_action_client.get_state() self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED) # but we can still overwrite coll = CollisionOperation() coll.object1 = "base_link" coll.object2 = coll.COLLISION_SET_OBJECTS coll.operation = coll.ENABLE goal.motion_plan_request.ordered_collision_operations.collision_operations.append( coll) goal.motion_plan_request.goal_constraints.joint_constraints[ 0].position = 0.0 goal.motion_plan_request.goal_constraints.joint_constraints[ 3].position = -0.2 goal.motion_plan_request.goal_constraints.joint_constraints[ 5].position = -0.2 self.move_arm_action_client.send_goal(goal) r = rospy.Rate(10) while True: cur_state = self.move_arm_action_client.get_state() if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and cur_state != actionlib_msgs.msg.GoalStatus.PENDING): break #should still have succeedeed final_state = self.move_arm_action_client.get_state() self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
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 "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)
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 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 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 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 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
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()