def check_validity(self, pr2_arms, arm): q = pr2_arms.get_joint_angles(arm) joint_nm_list = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] req = GetStateValidityRequest() req.robot_state.joint_state.name = joint_nm_list req.robot_state.joint_state.position = q req.robot_state.joint_state.header.stamp = rospy.Time.now() req.check_collisions = True res = self.state_validator.call(req) return res
def check_validity(self, pr2_arms, arm): q = pr2_arms.get_joint_angles(arm) joint_nm_list = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] req = GetStateValidityRequest() req.robot_state.joint_state.name = joint_nm_list req.robot_state.joint_state.position = q req.robot_state.joint_state.header.stamp = rospy.Time.now() req.check_collisions = True res = self.state_validator.call(req) return res
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
rospy.init_node('get_state_validity_python') srv_nm = 'environment_server_right_arm/get_state_validity' rospy.wait_for_service(srv_nm) get_state_validity = rospy.ServiceProxy(srv_nm, GetStateValidity) req = GetStateValidityRequest() req.robot_state.joint_state.name = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] req.robot_state.joint_state.position = [0.] * 7 req.robot_state.joint_state.position[0] = 0.4 req.robot_state.joint_state.position[3] = -0.4 req.robot_state.joint_state.header.stamp = rospy.Time.now() req.check_collisions = True res = get_state_validity.call(req) if res.error_code.val == res.error_code.SUCCESS: rospy.loginfo('Requested state is not in collision') else: rospy.loginfo('Requested state is in collision. Error code: %d'%(res.error_code.val))
from planning_environment_msgs.srv import GetStateValidity from planning_environment_msgs.srv import GetStateValidityRequest if __name__ == '__main__': rospy.init_node('get_state_validity_python') srv_nm = 'environment_server_right_arm/get_state_validity' rospy.wait_for_service(srv_nm) get_state_validity = rospy.ServiceProxy(srv_nm, GetStateValidity) req = GetStateValidityRequest() req.robot_state.joint_state.name = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] req.robot_state.joint_state.position = [0.] * 7 req.robot_state.joint_state.position[0] = 0.4 req.robot_state.joint_state.position[3] = -0.4 req.robot_state.joint_state.header.stamp = rospy.Time.now() req.check_collisions = True res = get_state_validity.call(req) if res.error_code.val == res.error_code.SUCCESS: rospy.loginfo('Requested state is not in collision') else: rospy.loginfo('Requested state is in collision. Error code: %d' % (res.error_code.val))