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 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