def test_get_base_state_validity(self): state_req = GetStateValidityRequest() state_req.check_collisions = True #empty state should be ok res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val != res.error_code.SUCCESS) #should be in collision state_req.robot_state.multi_dof_joint_state.joint_names.append( "base_joint") state_req.robot_state.multi_dof_joint_state.frame_ids.append( "odom_combined") state_req.robot_state.multi_dof_joint_state.child_frame_ids.append( "base_footprint") pose = Pose() pose.position.x = 4.0 pose.orientation.w = 1.0 state_req.robot_state.multi_dof_joint_state.poses.append(pose) res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val == res.error_code.SUCCESS) self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) #shouldn't be in collision as it doesn't have right parent_frame state_req.robot_state.multi_dof_joint_state.frame_ids[0] = "" res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val != res.error_code.SUCCESS) #should be in collision now state_req.robot_state.joint_state.name.append("floating_trans_x") state_req.robot_state.joint_state.position.append(4.0) res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val == res.error_code.SUCCESS) self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) #should be in collision now #moving back, but should still be in collision state_req.robot_state.joint_state.position[0] = 3.3 res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val == res.error_code.SUCCESS) self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) #but at a different theta should be ok state_req.robot_state.joint_state.name.append("floating_rot_z") state_req.robot_state.joint_state.position.append(.7071) state_req.robot_state.joint_state.name.append("floating_rot_w") state_req.robot_state.joint_state.position.append(.7071) res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val != res.error_code.SUCCESS)
def test_get_base_state_validity(self): state_req = GetStateValidityRequest() state_req.check_collisions = True #empty state should be ok res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val != res.error_code.SUCCESS) #should be in collision state_req.robot_state.multi_dof_joint_state.joint_names.append("base_joint") state_req.robot_state.multi_dof_joint_state.frame_ids.append("odom_combined") state_req.robot_state.multi_dof_joint_state.child_frame_ids.append("base_footprint") pose = Pose() pose.position.x = 4.0 pose.orientation.w = 1.0 state_req.robot_state.multi_dof_joint_state.poses.append(pose) res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val == res.error_code.SUCCESS) self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) #shouldn't be in collision as it doesn't have right parent_frame state_req.robot_state.multi_dof_joint_state.frame_ids[0] = "" res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val != res.error_code.SUCCESS) #should be in collision now state_req.robot_state.joint_state.name.append("floating_trans_x"); state_req.robot_state.joint_state.position.append(4.0); res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val == res.error_code.SUCCESS) self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) #should be in collision now #moving back, but should still be in collision state_req.robot_state.joint_state.position[0] = 3.3; res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val == res.error_code.SUCCESS) self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) #but at a different theta should be ok state_req.robot_state.joint_state.name.append("floating_rot_z"); state_req.robot_state.joint_state.position.append(.7071); state_req.robot_state.joint_state.name.append("floating_rot_w"); state_req.robot_state.joint_state.position.append(.7071); res = self.get_state_validity_server.call(state_req) self.failIf(res.error_code.val != res.error_code.SUCCESS)
def _create_state_validity_request(self): # set up a request message for state checks state_req = GetStateValidityRequest() state_req.check_collisions = True state_req.robot_state.joint_state.header.stamp = rospy.Time.now() # get the current joint state currState = self._current_joint_state() #print str(currState) if currState is None: return None # set the joint name and joint value arrays in the state check request message for name in currState.name: state_req.robot_state.joint_state.name.append(name) state_req.robot_state.joint_state.position.append(currState.position[currState.name.index(name)]) #print str(state_req) return state_req
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)
SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff" rospy.wait_for_service(SET_PLANNING_SCENE_DIFF_NAME) set_planning_scene_server = rospy.ServiceProxy(SET_PLANNING_SCENE_DIFF_NAME, SetPlanningSceneDiff) # Just send an empty message: res = set_planning_scene_server.call(SetPlanningSceneDiffRequest()) # One-time initialization of the state validity server: rospy.wait_for_service('/planning_scene_validity_server/get_state_validity') state_validity_server = rospy.ServiceProxy( '/planning_scene_validity_server/get_state_validity', GetStateValidity) # Set up a request message for state checks: state_req = GetStateValidityRequest() state_req.check_collisions = True state_req.robot_state.joint_state.header.stamp = rospy.Time.now() # -------------------------------------------- Usage ---------------------------------------- # Get the current joint state, which is presumably collision free: currState = currentJointState() print str(currState) # Set the joint name and joint value arrays in the state check request message # just for the right shoulder joints. The joint service uses the current joint # states for any unfilled joint fields in the request message: for name in currState.name: state_req.robot_state.joint_state.name.append(name) state_req.robot_state.joint_state.position.append(
# One-time initialization of the environment server: SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff" rospy.wait_for_service(SET_PLANNING_SCENE_DIFF_NAME); set_planning_scene_server = rospy.ServiceProxy(SET_PLANNING_SCENE_DIFF_NAME, SetPlanningSceneDiff) # Just send an empty message: res = set_planning_scene_server.call(SetPlanningSceneDiffRequest()) # One-time initialization of the state validity server: rospy.wait_for_service('/planning_scene_validity_server/get_state_validity'); state_validity_server = rospy.ServiceProxy('/planning_scene_validity_server/get_state_validity', GetStateValidity) # Set up a request message for state checks: state_req = GetStateValidityRequest() state_req.check_collisions = True state_req.robot_state.joint_state.header.stamp = rospy.Time.now() # -------------------------------------------- Usage ---------------------------------------- # Get the current joint state, which is presumably collision free: currState = currentJointState(); print str(currState) # Set the joint name and joint value arrays in the state check request message # just for the right shoulder joints. The joint service uses the current joint # states for any unfilled joint fields in the request message: for name in currState.name: state_req.robot_state.joint_state.name.append(name) state_req.robot_state.joint_state.position.append(currState.position[currState.name.index(name)])
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)