class Action(object):
def __init__(self):
super(Action, self).__init__()
self._duration = 3.0
self.execute_finished_signal = Signal()
def get_duration(self):
return self._duration
def set_duration(self, duration):
self._duration = duration
#@abstractmethod
def to_string(self):
pass
def deepcopy(self):
action = self.__class__()
action._duration = self._duration
return action
def serialize(self, stream):
stream.serialize_data(self._duration)
def deserialize(self, stream):
self._duration = stream.deserialize_data()
#@abstractmethod
def execute(self):
pass
def _execute_finished(self):
self.execute_finished_signal.emit()
class Ps3Subscriber(object):
top_button = 'top'
right_button = 'right'
bottom_button = 'bottom'
left_button = 'left'
select_button = 'select'
start_button = 'start'
square_button = 'square'
triangle_button = 'triangle'
cross_button = 'cross'
circle_button = 'circle'
def __init__(self):
self.buttons_changed = Signal()
self._buttons = None
self._pressed_buttons = set()
rospy.Subscriber('ps3_joy', Joy, self._joy_callback)
def _joy_callback(self, joy_msg):
if self._buttons != joy_msg.buttons:
self._buttons = joy_msg.buttons
self.buttons_changed.emit()
def get_triggered_buttons(self):
buttons = {
4: self.top_button,
5: self.right_button,
6: self.bottom_button,
7: self.left_button,
0: self.select_button,
3: self.start_button,
15: self.square_button,
12: self.triangle_button,
14: self.cross_button,
13: self.circle_button,
}
triggered = set()
for index, value in enumerate(self._buttons):
if index in buttons.keys():
index = buttons[index]
if value != 0:
if index not in self._pressed_buttons:
self._pressed_buttons.add(index)
triggered.add(index)
else:
if index in self._pressed_buttons:
self._pressed_buttons.remove(index)
return triggered
class KontrolSubscriber(object):
previous_button = 'previous'
play_button = 'play'
next_button = 'next'
repeat_button = 'repeat'
stop_button = 'stop'
record_button = 'record'
top1_button = 'top1'
bottom1_button = 'bottom1'
top2_button = 'top2'
bottom2_button = 'bottom2'
top6_button = 'top6'
bottom6_button = 'bottom6'
top7_button = 'top7'
bottom7_button = 'bottom7'
top8_button = 'top8'
bottom8_button = 'bottom8'
top9_button = 'top9'
bottom9_button = 'bottom9'
def __init__(self):
self.axes_changed = Signal()
self.buttons_changed = Signal()
self._axes = None
self._buttons = None
self._pressed_buttons = set()
self._last_larm = None
self._last_rarm = None
self._last_lgrip = None
self._last_rgrip = None
#************
rospy.Subscriber('korg_joy', Joy, self._joy_callback)
#rospy.Subscriber('/joy', Joy, self._joy_callback)
#************
def _joy_callback(self, joy_msg):
if self._axes != joy_msg.axes:
self._axes = joy_msg.axes
self.axes_changed.emit()
if self._buttons != joy_msg.buttons:
self._buttons = joy_msg.buttons
self.buttons_changed.emit()
def is_valid_action_set(self):
if self._buttons is None:
return False
if len(self._buttons) < 25:
return False
mode = self._buttons[24]
if mode == 3:
return False
return True
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
def get_joint_values(self):
values = {}
set = self.get_action_set()
for action in set._actions:
is_degree = isinstance(action, Pr2JointTrajectoryAction) and not isinstance(action, Pr2MoveTorsoAction)
for index, data in enumerate(action._joints):
name = data['label']
value = action._values[index]
if is_degree:
value = value * math.pi / 180.0
values[name] = value
return values
def get_triggered_buttons(self):
buttons = {
18: self.previous_button,
19: self.play_button,
20: self.next_button,
21: self.repeat_button,
22: self.stop_button,
23: self.record_button,
0: self.top1_button,
1: self.bottom1_button,
2: self.top2_button,
3: self.bottom2_button,
10: self.top6_button,
11: self.bottom6_button,
12: self.top7_button,
13: self.bottom7_button,
14: self.top8_button,
15: self.bottom8_button,
16: self.top9_button,
17: self.bottom9_button,
}
triggered = set()
for index, value in enumerate(self._buttons):
if index in buttons.keys():
index = buttons[index]
if value == 1:
if index not in self._pressed_buttons:
self._pressed_buttons.add(index)
triggered.add(index)
else:
if index in self._pressed_buttons:
self._pressed_buttons.remove(index)
return triggered
def _set_transformed_data(self, action, data):
assert(len(action._joints) == len(data))
transformed = []
for index, joint in enumerate(action._joints):
min_value = joint['min']
max_value = joint['max']
value = self._transform_value(data[index], min_value, max_value)
transformed.append(value)
action.set_values(transformed)
#print 'joints', action._joints
#print 'data', data
def _transform_value(self, value, min_value, max_value):
assert(value >= -1 and value <= 1)
value = (value + 1.0) / 2.0
value = min_value + value * (max_value - min_value)
assert(value >= min_value)
assert(value <= max_value)
return value
class KontrolSubscriber(object):
previous_button = 'previous'
play_button = 'play'
next_button = 'next'
repeat_button = 'repeat'
stop_button = 'stop'
record_button = 'record'
top1_button = 'top1'
bottom1_button = 'bottom1'
top2_button = 'top2'
bottom2_button = 'bottom2'
top6_button = 'top6'
bottom6_button = 'bottom6'
top7_button = 'top7'
bottom7_button = 'bottom7'
top8_button = 'top8'
bottom8_button = 'bottom8'
top9_button = 'top9'
bottom9_button = 'bottom9'
def __init__(self):
self.axes_changed = Signal()
self.buttons_changed = Signal()
self._axes = None
self._buttons = None
self._pressed_buttons = set()
self._last_larm = None
self._last_rarm = None
self._last_lgrip = None
self._last_rgrip = None
rospy.Subscriber('korg_joy', Joy, self._joy_callback)
def _joy_callback(self, joy_msg):
if self._axes != joy_msg.axes:
self._axes = joy_msg.axes
self.axes_changed.emit()
if self._buttons != joy_msg.buttons:
self._buttons = joy_msg.buttons
self.buttons_changed.emit()
def is_valid_action_set(self):
if self._buttons is None:
return False
if len(self._buttons) < 25:
return False
mode = self._buttons[24]
if mode == 3:
return False
return True
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
def get_joint_values(self):
values = {}
set = self.get_action_set()
for action in set._actions:
is_degree = isinstance(action, Pr2JointTrajectoryAction) and not isinstance(action, Pr2MoveTorsoAction)
for index, data in enumerate(action._joints):
name = data['label']
value = action._values[index]
if is_degree:
value = value * math.pi / 180.0
values[name] = value
return values
def get_triggered_buttons(self):
buttons = {
18: self.previous_button,
19: self.play_button,
20: self.next_button,
21: self.repeat_button,
22: self.stop_button,
23: self.record_button,
0: self.top1_button,
1: self.bottom1_button,
2: self.top2_button,
3: self.bottom2_button,
10: self.top6_button,
11: self.bottom6_button,
12: self.top7_button,
13: self.bottom7_button,
14: self.top8_button,
15: self.bottom8_button,
16: self.top9_button,
17: self.bottom9_button,
}
triggered = set()
for index, value in enumerate(self._buttons):
if index in buttons.keys():
index = buttons[index]
if value == 1:
if index not in self._pressed_buttons:
self._pressed_buttons.add(index)
triggered.add(index)
else:
if index in self._pressed_buttons:
self._pressed_buttons.remove(index)
return triggered
def _set_transformed_data(self, action, data):
assert(len(action._joints) == len(data))
transformed = []
for index, joint in enumerate(action._joints):
min_value = joint['min']
max_value = joint['max']
value = self._transform_value(data[index], min_value, max_value)
transformed.append(value)
action.set_values(transformed)
#print 'joints', action._joints
#print 'data', data
def _transform_value(self, value, min_value, max_value):
assert(value >= -1 and value <= 1)
value = (value + 1.0) / 2.0
value = min_value + value * (max_value - min_value)
assert(value >= min_value)
assert(value <= max_value)
return value
