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