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
import roslib; roslib.load_manifest('arm_navigation_tutorials')
import sys
import rospy
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)
import roslib
roslib.load_manifest('arm_navigation_tutorials')
import sys
import rospy
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:
