def insert_find_face():
model = get_current_model()
action_set = ActionSet()
action = Pr2LookAtFace()
action_set.add_action(action)
action_set.set_duration(main_window.duration_doubleSpinBox.value())
model.add_action(action_set)
def add_default_pose():
model = get_current_model()
action_set = ActionSet()
action = DefaultAction()
action_set.add_action(action)
action_set.set_duration(main_window.duration_doubleSpinBox.value())
model.add_action(action_set)
table_view = get_table_view(get_current_tab_index())
rows = len(model.action_sequence().actions())
table_view.resizeColumnsToContents()
table_view.selectRow(rows - 1)
class JointObserver(QObject):
current_values_changed = Signal(str)
_update_current_value_signal = Signal()
def __init__(self):
super(JointObserver, self).__init__()
self.action_set = None
self._latest_joint_state = None
self._update_current_value_signal.connect(self._update_current_value)
self._subscriber = None
self._trajectory_locks = []
self._trajectory_locks.append(TrajectoryLock('head_traj_controller_loose', [.08, .08]))
self._trajectory_locks.append(TrajectoryLock('l_arm_controller_loose', [.02, .02, .02, .02, .02, .06, .06]))
self._trajectory_locks.append(TrajectoryLock('r_arm_controller_loose', [.02, .02, .02, .02, .02, .06, .06]))
def start(self):
print 'JointObserver.start()'
if self._subscriber is not None:
self._subscriber.unregister()
self._subscriber = rospy.Subscriber('/joint_states', JointState, self._receive_joint_states)
for trajectory_lock in self._trajectory_locks:
trajectory_lock.start()
def stop(self):
print 'JointObserver.stop()'
if self._subscriber is not None:
self._subscriber.unregister()
self._subscriber = None
for trajectory_lock in self._trajectory_locks:
trajectory_lock.stop()
def _receive_joint_states(self, joint_state_msg):
self._latest_joint_state = joint_state_msg
self._update_current_value_signal.emit()
def _update_current_value(self):
self.action_set = ActionSet()
self.action_set.add_action(Pr2MoveHeadAction())
self.action_set.add_action(Pr2MoveLeftArmAction())
self.action_set.add_action(Pr2MoveRightArmAction())
labels = self.action_set.get_labels()
for label in labels:
value = self._latest_joint_state.position[self._latest_joint_state.name.index(label)]
value *= 180.0 / math.pi
try:
self.action_set.update_value(label, value)
except KeyError, e:
print 'JointObserver._update_current_value() label "%s" not found' % label
pass
value = self.action_set.to_string()
self.current_values_changed.emit(value)
def _update_current_value(self):
self.action_set = ActionSet()
self.action_set.add_action(Pr2MoveHeadAction())
self.action_set.add_action(Pr2MoveLeftArmAction())
self.action_set.add_action(Pr2MoveRightArmAction())
labels = self.action_set.get_labels()
for label in labels:
value = self._latest_joint_state.position[
self._latest_joint_state.name.index(label)]
value *= 180.0 / math.pi
try:
self.action_set.update_value(label, value)
except KeyError, e:
print 'JointObserver._update_current_value() label "%s" not found' % label
pass
def insert_find_face():
model = get_current_model()
action_set = ActionSet()
action = Pr2LookAtFace()
action_set.add_action(action)
action_set.set_duration(main_window.duration_doubleSpinBox.value())
model.add_action(action_set)
def _update_current_value(self):
self.action_set = ActionSet()
self.action_set.add_action(Pr2MoveHeadAction())
self.action_set.add_action(Pr2MoveLeftArmAction())
self.action_set.add_action(Pr2MoveRightArmAction())
labels = self.action_set.get_labels()
for label in labels:
value = self._latest_joint_state.position[self._latest_joint_state.name.index(label)]
value *= 180.0 / math.pi
try:
self.action_set.update_value(label, value)
except KeyError, e:
print 'JointObserver._update_current_value() label "%s" not found' % label
pass
def add_default_pose():
model = get_current_model()
action_set = ActionSet()
action = DefaultAction()
action_set.add_action(action)
action_set.set_duration(main_window.duration_doubleSpinBox.value())
model.add_action(action_set)
table_view = get_table_view(get_current_tab_index())
rows = len(model.action_sequence().actions())
table_view.resizeColumnsToContents()
table_view.selectRow(rows - 1)
class JointObserver(QObject):
current_values_changed = Signal(str)
_update_current_value_signal = Signal()
def __init__(self):
super(JointObserver, self).__init__()
self.action_set = None
self._latest_joint_state = None
self._update_current_value_signal.connect(self._update_current_value)
self._subscriber = None
self._trajectory_locks = []
self._trajectory_locks.append(
TrajectoryLock('head_traj_controller_loose', [.08, .08]))
self._trajectory_locks.append(
TrajectoryLock('l_arm_controller_loose',
[.02, .02, .02, .02, .02, .06, .06]))
self._trajectory_locks.append(
TrajectoryLock('r_arm_controller_loose',
[.02, .02, .02, .02, .02, .06, .06]))
def start(self):
print 'JointObserver.start()'
if self._subscriber is not None:
self._subscriber.unregister()
self._subscriber = rospy.Subscriber('/joint_states', JointState,
self._receive_joint_states)
for trajectory_lock in self._trajectory_locks:
trajectory_lock.start()
def stop(self):
print 'JointObserver.stop()'
if self._subscriber is not None:
self._subscriber.unregister()
self._subscriber = None
for trajectory_lock in self._trajectory_locks:
trajectory_lock.stop()
def _receive_joint_states(self, joint_state_msg):
self._latest_joint_state = joint_state_msg
self._update_current_value_signal.emit()
def _update_current_value(self):
self.action_set = ActionSet()
self.action_set.add_action(Pr2MoveHeadAction())
self.action_set.add_action(Pr2MoveLeftArmAction())
self.action_set.add_action(Pr2MoveRightArmAction())
labels = self.action_set.get_labels()
for label in labels:
value = self._latest_joint_state.position[
self._latest_joint_state.name.index(label)]
value *= 180.0 / math.pi
try:
self.action_set.update_value(label, value)
except KeyError, e:
print 'JointObserver._update_current_value() label "%s" not found' % label
pass
value = self.action_set.to_string()
self.current_values_changed.emit(value)
def get_action_set(self):
set = ActionSet()
if self._axes is None or self._buttons is None:
# default action to test without hardware slider
return DefaultAction()
if not self.is_valid_action_set():
return None
mode = self._buttons[24]
head = Pr2MoveHeadAction()
head_data = [-self._axes[9], -self._axes[0]]
self._set_transformed_data(head, head_data)
set.add_action(head)
torso = Pr2MoveTorsoAction()
torso_data = [self._axes[8]]
self._set_transformed_data(torso, torso_data)
set.add_action(torso)
if mode != 3:
larm_data = []
larm_data.extend(self._axes[1:7])
larm_data.append(self._axes[16])
larm_data[0] = -larm_data[0]
larm_data[1] = -larm_data[1]
larm_data[2] = -larm_data[2]
larm_data[4] = -larm_data[4]
larm_data[6] = -larm_data[6]
rarm_data = []
rarm_data.extend(self._axes[1:7])
rarm_data.append(self._axes[16])
rarm_data[1] = -rarm_data[1]
lgrip_data = [self._axes[7]]
rgrip_data = [lgrip_data[0]]
if mode == 0 or mode == 1:
larm = Pr2MoveLeftArmAction()
self._set_transformed_data(larm, larm_data)
set.add_action(larm)
self._last_larm = larm
lgrip = Pr2MoveLeftGripperAction()
self._set_transformed_data(lgrip, lgrip_data)
set.add_action(lgrip)
self._last_lgrip = lgrip
elif mode == 2:
if self._last_larm is not None:
set.add_action(self._last_larm.deepcopy())
if self._last_lgrip is not None:
set.add_action(self._last_lgrip.deepcopy())
if mode == 0 or mode == 2:
rarm = Pr2MoveRightArmAction()
self._set_transformed_data(rarm, rarm_data)
set.add_action(rarm)
self._last_rarm = rarm
rgrip = Pr2MoveRightGripperAction()
self._set_transformed_data(rgrip, rgrip_data)
set.add_action(rgrip)
self._last_rgrip = rgrip
elif mode == 1:
if self._last_rarm is not None:
set.add_action(self._last_rarm.deepcopy())
if self._last_rgrip is not None:
set.add_action(self._last_rgrip.deepcopy())
duration = self._transform_value(self._axes[17], 0.5, 5.0)
set.set_duration(duration)
return set
action5 = WaitAction(3.0) seq.add_action(action5) action6 = Pr2MoveLeftGripperAction() action6.set_value(0.0) seq.add_action(action6) action7 = Pr2MoveLeftGripperAction() action7.set_value(0.08) seq.add_action(action7) action8 = Pr2MoveHeadAction() action8.set_values([-60, -30]) action9 = Pr2MoveTorsoAction() action9.set_values([0.0]) set1 = ActionSet() set1.add_action(action8) set1.add_action(action9) seq.add_action(set1) action10 = Pr2MoveHeadAction() action10.set_values([60, 30]) action11 = Pr2MoveTorsoAction() action11.set_values([0.3]) set2 = ActionSet() set2.add_action(action10) set2.add_action(action11) seq.add_action(set2) action5 = WaitAction(3.0) seq.add_action(action5)
def scaled_pose_to_actionset(pose):
""" Takes the given museum_msgs/Pose message and converts it into an ActionSet
It is assumed that pose values are already scaled """
head = Pr2MoveHeadAction()
head.set_values([pose.head.head_pan, pose.head.head_tilt])
torso = Pr2MoveTorsoAction()
torso.set_values([pose.torso])
def arm_data(arm):
return [
arm.shoulder_pan,
arm.shoulder_lift,
arm.upper_arm_roll,
arm.elbow_flex,
arm.forearm_roll,
arm.wrist_flex,
arm.wrist_roll
]
larm = Pr2MoveLeftArmAction()
larm.set_values(arm_data(pose.left))
lgrip = Pr2MoveLeftGripperAction()
lgrip.set_values([pose.left.gripper])
rarm = Pr2MoveRightArmAction()
rarm.set_values(arm_data(pose.right))
rgrip = Pr2MoveRightGripperAction()
rgrip.set_values([pose.right.gripper])
duration = pose.duration
actionset = ActionSet()
actionset.add_action(head)
actionset.add_action(torso)
actionset.add_action(larm)
actionset.add_action(lgrip)
actionset.add_action(rarm)
actionset.add_action(rgrip)
actionset.set_duration(duration)
return actionset
def pose_to_actionset(pose):
""" Takes the given museum_msgs/Pose message and converts it into an ActionSet
It is assumed that pose values are between 0 and 1. They will be scaled """
head = Pr2MoveHeadAction()
head_vals = [pose.head.head_pan, pose.head.head_tilt]
_set_transformed_data(head, head_vals)
torso = Pr2MoveTorsoAction()
torso_vals = [pose.torso]
_set_transformed_data(torso, torso_vals)
def arm_data(arm):
return [
arm.shoulder_pan,
arm.shoulder_lift,
arm.upper_arm_roll,
arm.elbow_flex,
arm.forearm_roll,
arm.wrist_flex,
arm.wrist_roll
]
larm = Pr2MoveLeftArmAction()
larm_vals = arm_data(pose.left)
_set_transformed_data(larm, larm_vals)
lgrip = Pr2MoveLeftGripperAction()
lgrip_vals = [pose.left.gripper]
_set_transformed_data(lgrip, lgrip_vals)
rarm = Pr2MoveRightArmAction()
rarm_vals = arm_data(pose.right)
_set_transformed_data(rarm, rarm_vals)
rgrip = Pr2MoveRightGripperAction()
rgrip_vals = [pose.right.gripper]
_set_transformed_data(rgrip, rgrip_vals)
actionset = ActionSet()
actionset.add_action(head)
actionset.add_action(torso)
actionset.add_action(larm)
actionset.add_action(lgrip)
actionset.add_action(rarm)
actionset.add_action(rgrip)
actionset.set_duration(_transform_value(pose.duration, 0.5, 5.0))
print actionset.to_string()
return actionset
def scaled_pose_to_actionset(pose):
""" Takes the given museum_msgs/Pose message and converts it into an ActionSet
It is assumed that pose values are already scaled """
head = Pr2MoveHeadAction()
head.set_values([pose.head.head_pan, pose.head.head_tilt])
torso = Pr2MoveTorsoAction()
torso.set_values([pose.torso])
def arm_data(arm):
return [
arm.shoulder_pan, arm.shoulder_lift, arm.upper_arm_roll,
arm.elbow_flex, arm.forearm_roll, arm.wrist_flex, arm.wrist_roll
]
larm = Pr2MoveLeftArmAction()
larm.set_values(arm_data(pose.left))
lgrip = Pr2MoveLeftGripperAction()
lgrip.set_values([pose.left.gripper])
rarm = Pr2MoveRightArmAction()
rarm.set_values(arm_data(pose.right))
rgrip = Pr2MoveRightGripperAction()
rgrip.set_values([pose.right.gripper])
duration = pose.duration
actionset = ActionSet()
actionset.add_action(head)
actionset.add_action(torso)
actionset.add_action(larm)
actionset.add_action(lgrip)
actionset.add_action(rarm)
actionset.add_action(rgrip)
actionset.set_duration(duration)
return actionset
def pose_to_actionset(pose):
""" Takes the given museum_msgs/Pose message and converts it into an ActionSet
It is assumed that pose values are between 0 and 1. They will be scaled """
head = Pr2MoveHeadAction()
head_vals = [pose.head.head_pan, pose.head.head_tilt]
_set_transformed_data(head, head_vals)
torso = Pr2MoveTorsoAction()
torso_vals = [pose.torso]
_set_transformed_data(torso, torso_vals)
def arm_data(arm):
return [
arm.shoulder_pan, arm.shoulder_lift, arm.upper_arm_roll,
arm.elbow_flex, arm.forearm_roll, arm.wrist_flex, arm.wrist_roll
]
larm = Pr2MoveLeftArmAction()
larm_vals = arm_data(pose.left)
_set_transformed_data(larm, larm_vals)
lgrip = Pr2MoveLeftGripperAction()
lgrip_vals = [pose.left.gripper]
_set_transformed_data(lgrip, lgrip_vals)
rarm = Pr2MoveRightArmAction()
rarm_vals = arm_data(pose.right)
_set_transformed_data(rarm, rarm_vals)
rgrip = Pr2MoveRightGripperAction()
rgrip_vals = [pose.right.gripper]
_set_transformed_data(rgrip, rgrip_vals)
actionset = ActionSet()
actionset.add_action(head)
actionset.add_action(torso)
actionset.add_action(larm)
actionset.add_action(lgrip)
actionset.add_action(rarm)
actionset.add_action(rgrip)
actionset.set_duration(_transform_value(pose.duration, 0.5, 5.0))
print actionset.to_string()
return actionset
def get_action_set(self):
set = ActionSet()
if self._axes is None or self._buttons is None:
# default action to test without hardware slider
return DefaultAction()
if not self.is_valid_action_set():
return None
mode = self._buttons[24]
head = Pr2MoveHeadAction()
head_data = [-self._axes[9], -self._axes[0]]
self._set_transformed_data(head, head_data)
set.add_action(head)
torso = Pr2MoveTorsoAction()
torso_data = [self._axes[8]]
self._set_transformed_data(torso, torso_data)
set.add_action(torso)
if mode != 3:
larm_data = []
larm_data.extend(self._axes[1:7])
larm_data.append(self._axes[16])
larm_data[0] = -larm_data[0]
larm_data[1] = -larm_data[1]
larm_data[2] = -larm_data[2]
larm_data[4] = -larm_data[4]
larm_data[6] = -larm_data[6]
rarm_data = []
rarm_data.extend(self._axes[1:7])
rarm_data.append(self._axes[16])
rarm_data[1] = -rarm_data[1]
lgrip_data = [self._axes[7]]
rgrip_data = [lgrip_data[0]]
if mode == 0 or mode == 1:
larm = Pr2MoveLeftArmAction()
self._set_transformed_data(larm, larm_data)
set.add_action(larm)
self._last_larm = larm
lgrip = Pr2MoveLeftGripperAction()
self._set_transformed_data(lgrip, lgrip_data)
set.add_action(lgrip)
self._last_lgrip = lgrip
elif mode == 2:
if self._last_larm is not None:
set.add_action(self._last_larm.deepcopy())
if self._last_lgrip is not None:
set.add_action(self._last_lgrip.deepcopy())
if mode == 0 or mode == 2:
rarm = Pr2MoveRightArmAction()
self._set_transformed_data(rarm, rarm_data)
set.add_action(rarm)
self._last_rarm = rarm
rgrip = Pr2MoveRightGripperAction()
self._set_transformed_data(rgrip, rgrip_data)
set.add_action(rgrip)
self._last_rgrip = rgrip
elif mode == 1:
if self._last_rarm is not None:
set.add_action(self._last_rarm.deepcopy())
if self._last_rgrip is not None:
set.add_action(self._last_rgrip.deepcopy())
duration = self._transform_value(self._axes[17], 0.5, 5.0)
set.set_duration(duration)
return set