def _move_out_of_collision(self, move_mag=0.3, num_tries=100): ''' Tries to find a small movement that will take the arm out of collision. **Args:** *move_mag (float):* Max magnitude in radians of movement for each joint. *num_tries (int):* Number of random joint angles to try before giving up. **Returns:** succeeded (boolean): True if arm was sucessfully moved out of collision. ''' req = GetStateValidityRequest() req.robot_state = self.world_interface.get_robot_state() req.check_collisions = True req.check_path_constraints = False req.check_joint_limits = False req.group_name = self._arm_name res = self._planner.get_state_validity_service(req) if res.error_code.val == ArmNavErrorCodes.SUCCESS: rospy.logdebug('Current state not in collision') return False joint_state = self._planner.arm_joint_state() current_joint_position = np.array(joint_state.position) for ii in range(num_tries): joint_position = current_joint_position + np.random.uniform( -move_mag, move_mag, (len(joint_state.position), )) joint_state.position = list(joint_position) trajectory_tools.set_joint_state_in_robot_state( joint_state, req.robot_state) res = self._planner.get_state_validity_service(req) in_collision = (res.error_code.val != ArmNavErrorCodes.SUCCESS) rospy.logdebug('%s in collision: %s' % (str(joint_position), str(in_collision))) if not in_collision: self._move_to_goal_directly(joint_state, None, None, collision_aware=False) self._current_handle._set_reached_goal(True) self._current_handle._set_reached_goal(False)
def is_current_state_in_collision(self, arm_name, check_joint_limits = False): ''' Tells you whether the current arm pose is in collision. **Returns:** succeeded (boolean): True if arm is in collision. ''' req = GetStateValidityRequest() req.robot_state = self._world_interface.get_robot_state() req.check_collisions = True req.check_path_constraints = False req.check_joint_limits = check_joint_limits req.group_name = arm_name res = self._planners[arm_name].get_state_validity_service(req) if res.error_code.val == ArmNavErrorCodes.SUCCESS: rospy.logdebug('Current state not in collision') return False else: rospy.logdebug('Current state in collision') return True
def check_state_validity(self, joint_angles, ordered_collision_operations = None, \ robot_state = None, link_padding = None): req = GetStateValidityRequest() if robot_state != None: req.robot_state = robot_state req.robot_state.joint_state.name.extend(self.joint_names) req.robot_state.joint_state.position.extend(joint_angles) req.robot_state.joint_state.header.stamp = rospy.Time.now() req.check_collisions = True if ordered_collision_operations != None: req.ordered_collision_operations = ordered_collision_operations if link_padding != None: req.link_padding = link_padding try: res = self._check_state_validity_service(req) except rospy.ServiceException, e: rospy.logwarn("Check state validity call failed! error msg: %s"%e) return 0
def test_alter_padding(self): req = GetRobotStateRequest() cur_state = self.get_robot_state_server.call(req) state_req = GetStateValidityRequest() state_req.robot_state = cur_state.robot_state state_req.check_collisions = True res = self.get_state_validity_server.call(state_req) #no big padding initially self.failIf(res.error_code.val != res.error_code.SUCCESS) padd = LinkPadding() padd.link_name = "r_end_effector" padd.padding = .03 state_req.link_padding = [] state_req.link_padding.append(padd) res = self.get_state_validity_server.call(state_req) #should be in collision self.failIf(res.error_code.val == res.error_code.SUCCESS) self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) #should be some contacts self.failIf(len(res.contacts) == 0) #check that revert works state_req.link_padding = [] res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val != res.error_code.SUCCESS)
def check_state_validity(self, joint_angles, ordered_collision_operations = None, \ robot_state = None, link_padding = None): req = GetStateValidityRequest() if robot_state != None: req.robot_state = robot_state req.robot_state.joint_state.name.extend(self.joint_names) req.robot_state.joint_state.position.extend(joint_angles) req.robot_state.joint_state.header.stamp = rospy.Time.now() req.check_collisions = True if ordered_collision_operations != None: req.ordered_collision_operations = ordered_collision_operations if link_padding != None: req.link_padding = link_padding try: res = self._check_state_validity_service(req) except rospy.ServiceException, e: rospy.logwarn("Check state validity call failed! error msg: %s" % e) return 0
def is_in_collision(self, arm_name, joint_state, check_joint_limits=False): ''' Tells you whether the current arm pose is in collision. **Returns:** succeeded (boolean): True if arm is in collision. ''' req = GetStateValidityRequest() req.robot_state = self._world_interface.get_robot_state() trajectory_tools.set_joint_state_in_robot_state( joint_state, req.robot_state) req.check_collisions = True req.check_path_constraints = False req.check_joint_limits = check_joint_limits req.group_name = self._arm_name res = self._planners[arm_name].get_state_validity_service(req) if res.error_code.val == ArmNavErrorCodes.SUCCESS: rospy.logdebug('Current state not in collision') return False else: rospy.logdebug('Current state in collision') return True
def _move_out_of_collision(self, move_mag=0.3, num_tries=100): ''' Tries to find a small movement that will take the arm out of collision. **Args:** *move_mag (float):* Max magnitude in radians of movement for each joint. *num_tries (int):* Number of random joint angles to try before giving up. **Returns:** succeeded (boolean): True if arm was sucessfully moved out of collision. ''' req = GetStateValidityRequest() req.robot_state = self.world_interface.get_robot_state() req.check_collisions = True req.check_path_constraints = False req.check_joint_limits = False req.group_name = self._arm_name res = self._planner.get_state_validity_service(req) if res.error_code.val == ArmNavErrorCodes.SUCCESS: rospy.logdebug('Current state not in collision') return False joint_state = self._planner.arm_joint_state() current_joint_position = np.array(joint_state.position) for ii in range(num_tries): joint_position = current_joint_position + np.random.uniform( -move_mag, move_mag, (len(joint_state.position),)) joint_state.position = list(joint_position) trajectory_tools.set_joint_state_in_robot_state(joint_state, req.robot_state) res = self._planner.get_state_validity_service(req) in_collision = (res.error_code.val != ArmNavErrorCodes.SUCCESS) rospy.logdebug('%s in collision: %s' % (str(joint_position), str(in_collision))) if not in_collision: self._move_to_goal_directly(joint_state, None, None, collision_aware=False) self._current_handle._set_reached_goal(True) self._current_handle._set_reached_goal(False)
def test_get_state_validity(self): req = GetRobotStateRequest() cur_state = self.get_robot_state_server.call(req) #for i in range(len(cur_state.robot_state.joint_state.name)): # print cur_state.robot_state.joint_state.name[i], cur_state.robot_state.joint_state.position[i] state_req = GetStateValidityRequest() state_req.robot_state = cur_state.robot_state group_req = GetGroupInfoRequest() group_req.group_name = 'right_arm' group_res = self.get_group_info_server.call(group_req) self.failIf(len(group_res.link_names) == 0) right_arm_links = group_res.link_names group_req.group_name = '' group_res = self.get_group_info_server.call(group_req) self.failIf(len(group_res.link_names) == 0) state_req.ordered_collision_operations.collision_operations = arm_navigation_msgs_utils.make_disable_allowed_collisions_with_exclusions(group_res.link_names, right_arm_links) for i in state_req.ordered_collision_operations.collision_operations: print 'Disabling ', i.object1 state_req.check_collisions = True res = self.get_state_validity_server.call(state_req) #should be in collision self.failIf(res.error_code.val == res.error_code.SUCCESS) self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) #should be some contacts self.failIf(len(res.contacts) == 0) for c in res.contacts: #getting everything in common frame of base_link contact_point = PointStamped() contact_point.header = c.header contact_point.point = c.position contact_point_base = self.tf.transformPoint("base_link",contact_point) gripper_point = PointStamped() gripper_point.header.stamp = c.header.stamp gripper_point.header.frame_id = "r_gripper_palm_link" gripper_point.point.x = 0 gripper_point.point.y = 0 gripper_point.point.z = 0 gripper_point_base = self.tf.transformPoint("base_link", gripper_point) print 'x diff:', abs(gripper_point_base.point.x-contact_point_base.point.x) print 'y diff:', abs(gripper_point_base.point.y-contact_point_base.point.y) print 'z diff:', abs(gripper_point_base.point.z-contact_point_base.point.z) self.failIf(abs(gripper_point_base.point.x-contact_point_base.point.x) > .031) self.failIf(abs(gripper_point_base.point.y-contact_point_base.point.y) > .031) self.failIf(abs(gripper_point_base.point.z-contact_point_base.point.z) > .031) #now we delete obj1 obj2 = CollisionObject() obj2.header.stamp = rospy.Time.now() obj2.header.frame_id = "base_link" obj2.id = "test_object" obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE self.obj_pub.publish(obj2) rospy.sleep(5.) cur_state = self.get_robot_state_server.call(req) state_req.robot_state = cur_state.robot_state res = self.get_state_validity_server.call(state_req) # base shouldn't collide due to child only links self.failIf(res.error_code.val != res.error_code.SUCCESS) # now it should collide state_req.ordered_collision_operations.collision_operations = [] res = self.get_state_validity_server.call(state_req) # base shouldn't collide due to child only links self.failIf(res.error_code.val == res.error_code.SUCCESS)
def test_get_state_validity(self): req = GetRobotStateRequest() cur_state = self.get_robot_state_server.call(req) #for i in range(len(cur_state.robot_state.joint_state.name)): # print cur_state.robot_state.joint_state.name[i], cur_state.robot_state.joint_state.position[i] state_req = GetStateValidityRequest() state_req.robot_state = cur_state.robot_state group_req = GetGroupInfoRequest() group_req.group_name = 'right_arm' group_res = self.get_group_info_server.call(group_req) self.failIf(len(group_res.link_names) == 0) right_arm_links = group_res.link_names group_req.group_name = '' group_res = self.get_group_info_server.call(group_req) self.failIf(len(group_res.link_names) == 0) state_req.ordered_collision_operations.collision_operations = arm_navigation_msgs_utils.make_disable_allowed_collisions_with_exclusions( group_res.link_names, right_arm_links) for i in state_req.ordered_collision_operations.collision_operations: print 'Disabling ', i.object1 state_req.check_collisions = True res = self.get_state_validity_server.call(state_req) #should be in collision self.failIf(res.error_code.val == res.error_code.SUCCESS) self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) #should be some contacts self.failIf(len(res.contacts) == 0) for c in res.contacts: #getting everything in common frame of base_link contact_point = PointStamped() contact_point.header = c.header contact_point.point = c.position contact_point_base = self.tf.transformPoint( "base_link", contact_point) gripper_point = PointStamped() gripper_point.header.stamp = c.header.stamp gripper_point.header.frame_id = "r_gripper_palm_link" gripper_point.point.x = 0 gripper_point.point.y = 0 gripper_point.point.z = 0 gripper_point_base = self.tf.transformPoint( "base_link", gripper_point) print 'x diff:', abs(gripper_point_base.point.x - contact_point_base.point.x) print 'y diff:', abs(gripper_point_base.point.y - contact_point_base.point.y) print 'z diff:', abs(gripper_point_base.point.z - contact_point_base.point.z) self.failIf( abs(gripper_point_base.point.x - contact_point_base.point.x) > .031) self.failIf( abs(gripper_point_base.point.y - contact_point_base.point.y) > .031) self.failIf( abs(gripper_point_base.point.z - contact_point_base.point.z) > .031) #now we delete obj1 obj2 = CollisionObject() obj2.header.stamp = rospy.Time.now() obj2.header.frame_id = "base_link" obj2.id = "test_object" obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE self.obj_pub.publish(obj2) rospy.sleep(5.) cur_state = self.get_robot_state_server.call(req) state_req.robot_state = cur_state.robot_state res = self.get_state_validity_server.call(state_req) # base shouldn't collide due to child only links self.failIf(res.error_code.val != res.error_code.SUCCESS) # now it should collide state_req.ordered_collision_operations.collision_operations = [] res = self.get_state_validity_server.call(state_req) # base shouldn't collide due to child only links self.failIf(res.error_code.val == res.error_code.SUCCESS)