def getStateValidity(self,
robot_state,
group_name='both_arms_torso',
constraints=None,
print_depth=False):
"""Given a RobotState and a group name and an optional Constraints
return the validity of the State"""
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
# Uncomment below to "pad" obstacles by only returning inCollision=True when
# max penetration depth is above some threshold
# invalid_depth = 0.0001
# if (not result.valid):
# contact_depths = []
# for i in range(len(result.contacts)):
# contact_depths.append(result.contacts[i].depth)
# max_depth = max(contact_depths)
# if max_depth < invalid_depth:
# return True
return result.valid #, max_depth, min_depth
def stateValidate(robot_state,
group_name,
constraints=None,
print_depth=False):
sv_srv = rospy.ServiceProxy(DEFAULT_SV_SERVICE, GetStateValidity)
#rospy.loginfo("Collision Checking by using check_state_validity service...")
#rospy.wait_for_service(STATE_VALID)
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = sv_srv.call(gsvr)
if (not result.valid):
contact_depths = []
for i in xrange(len(result.contacts)):
contact_depths.append(result.contacts[i].depth)
max_depth = max(contact_depths)
if max_depth < 0.0001:
return True
else:
return False
return result.valid
def getStateValidity(self, robot_state, group_name='both_arms_torso', constraints=None):
"""Given a RobotState and a group name and an optional Constraints
return the validity of the State"""
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
#GetStateValidityResponse()
#rospy.logwarn("sent: " + str(gsvr))
return result
def getStateValidity(self, robot_state, group_name='both_arms_torso', constraints=None):
"""Given a RobotState and a group name and an optional Constraints
return the validity of the State"""
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
# if not result.valid:
# print("The contact information are: %s" % result.contacts)
# print("After checking for state validity. Result: %s" % result.valid)
return result
def getStateValidity(self, group_name='acrobat', constraints=None):
'''
Given a RobotState and a group name and an optional Constraints
return the validity of the State
'''
gsvr = GetStateValidityRequest()
gsvr.robot_state = self.rs
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
print(result)
return result
def getStateValidity(self,
robot_state,
group_name='both_arms_torso',
constraints=None):
"""Given a RobotState and a group name and an optional Constraints
return the validity of the State"""
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
return result.valid
def getStateValidity(self, robot_state, group_name='both_arms_torso', constraints=None):
"""Given a RobotState and a group name and an optional Constraints
return the validity of the State"""
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
# GetStateValidityResponse()
# rospy.logwarn("sent: " + str(gsvr))
# JAN BEHRENS ([email protected] - 2018-10-30): Return a bool instead of the full message
return result.valid
def getStateValidity(self, robot_state, group_name='both_arms_torso', constraints=None, print_depth=False):
"""Given a RobotState and a group name and an optional Constraints
return the validity of the State"""
gsvr = GetStateValidityRequest()
gsvr.robot_state = robot_state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
result = self.sv_srv.call(gsvr)
if (not result.valid):
contact_depths = []
for i in range(len(result.contacts)):
contact_depths.append(result.contacts[i].depth)
max_depth = max(contact_depths)
if max_depth < 0.0001:
return True
else:
return False
return result.valid
def getStateValidity(self, group_name='ur5', constraints=None):
'''
Given a RobotState and a group name and an optional Constraints
return the validity of the State
'''
gsvr = GetStateValidityRequest()
gsvr.robot_state = self.rs.state
gsvr.group_name = group_name
if constraints != None:
gsvr.constraints = constraints
print("====================================")
print(self.rs.state.joint_state.position)
result = self.sv_srv.call(gsvr)
print('result:')
print('valid = {}'.format(result.valid))
rc = result.contacts
for i in range(len(rc)):
print('contact {}: 1-{}, 2-{}'.format(i, rc[i].contact_body_1,
rc[i].contact_body_2))
return result