def Decide(self):
dist = dist_between_links.check("object", "bert2::left_wrist_flex_link")
if dist <= 0.1:
rospy.loginfo("Valid assertion")
return 1
else:
rospy.loginfo("Violation of property")
return -1
def Decide(self):
dist = dist_between_links.check('object',
'bert2::left_wrist_flex_link')
if dist <= 0.1:
rospy.loginfo('Valid assertion')
return 1
else:
rospy.loginfo('Violation of property')
return -1
def distCheck(self):
# Just check the distances from the human hand to bert2's main left arm links:
# N.B. For greater realism, we should check the shortest distance between the outer bounds of each link,
# rather than just checking the distance between nominal points on the link.
for bert2link in ['bert2::left_humeral_rot_link','bert2::left_elbow_flex_link','bert2::left_wrist_flex_link']:
dist = check('human_hand::human_hand_link','bert2link')
if dist<=self.distThresh:
return 1
else:
return 0
def Decide(self):
dist = dist_between_links.check('object','bert2::left_wrist_flex_link')
self.object_in_robot_hand = dist<=0.1
if not object_in_robot_hand:
rospy.loginfo('Valid assertion')
return 1
else:
rospy.loginfo('Violation of property')
return -1
def precon(self):
dist = dist_between_links.check('object','bert2::left_wrist_flex_link')
self.object_in_robot_hand = dist<=0.1
if self.h_ready==1 and self.sensorsOK and self.object_in_robot_hand:
self.t_WaitStart = rospy.get_time()
self.object_hand = 0 # In case it's been set prematurely.
self.decision = 0 # In case it's been set prematurely.
return 1
else:
return 0
def Decide(self):
dist = dist_between_links.check('object',
'bert2::left_wrist_flex_link')
self.object_in_robot_hand = dist <= 0.1
if not object_in_robot_hand:
rospy.loginfo('Valid assertion')
return 1
else:
rospy.loginfo('Violation of property')
return -1
def precon(self):
dist = dist_between_links.check('object',
'bert2::left_wrist_flex_link')
self.object_in_robot_hand = dist <= 0.1
if self.h_ready == 1 and self.sensorsOK and self.object_in_robot_hand:
self.t_WaitStart = rospy.get_time()
self.object_hand = 0 # In case it's been set prematurely.
self.decision = 0 # In case it's been set prematurely.
return 1
else:
return 0
def precon(self):
dist = dist_between_links.check("object_link", "bert2::left_wrist_flex_link")
obj_in_rob_hand = dist <= 0.1
# if (not self.sensorsOK) and obj_in_rob_hand:
if self.h_ready == 1 and (not self.sensorsOK) and obj_in_rob_hand:
self.t_WaitStart = rospy.get_time()
self.object_hand = 1 # In case it's been set prematurely.
self.decision = 0 # In case it's been set prematurely.
return 1
else:
return 0
def distCheck(self):
# Just check the distances from the human hand to bert2's main left arm links:
# N.B. For greater realism, we should check the shortest distance between the outer bounds of each link,
# rather than just checking the distance between nominal points on the link.
for bert2link in [
'bert2::left_humeral_rot_link', 'bert2::left_elbow_flex_link',
'bert2::left_wrist_flex_link'
]:
dist = check('human_hand::human_hand_link', 'bert2link')
if dist <= self.distThresh:
return 1
else:
return 0