class MultiDofInput(avango.script.Script):
### fields ###
## output fields
mf_dof = avango.MFFloat()
mf_dof.value = [0.0,0.0,0.0,0.0,0.0,0.0,0.0] # init 7 channels
mf_buttons = avango.MFBool()
mf_buttons.value = [False, False, False] # init 3 buttons
### functions ###
def filter_channel(self, VALUE, OFFSET, MIN, MAX, NEG_THRESHOLD, POS_THRESHOLD):
VALUE = VALUE - OFFSET
MIN = MIN - OFFSET
MAX = MAX - OFFSET
if VALUE > 0:
_pos = MAX * POS_THRESHOLD * 0.01
if VALUE > _pos: # above positive threshold
VALUE = min( (VALUE - _pos) / (MAX - _pos), 1.0) # normalize interval
else: # below positive threshold
VALUE = 0
elif VALUE < 0:
_neg = MIN * NEG_THRESHOLD * 0.01
if VALUE < _neg:
VALUE = max( (VALUE - _neg) / abs(MIN - _neg), -1.0)
else: # above negative threshold
VALUE = 0
return VALUE
class MultiDofInput(avango.script.Script):
### fields ###
## output fields
mf_dof = avango.MFFloat()
mf_dof.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # init 7 channels
mf_buttons = avango.MFBool()
mf_buttons.value = [False, False, False] # init 3 buttons
## Map an input value to a certain interval.
# @param VALUE The value to be mapped.
# @param OFFSET The offset to be applied to VALUE, MIN and MAX.
# @param MIN The minimum value of the old interval.
# @param MAX The maximum value of the old interval.
# @param NEG_THRESHOLD The negative threshold to be used.
# @param POS_THRESHOLD The positive threshold to be used.
def filter_channel(self, VALUE, OFFSET, MIN, MAX, NEG_THRESHOLD,
POS_THRESHOLD):
VALUE -= OFFSET
MIN -= OFFSET
MAX -= OFFSET
#print("+", VALUE, MAX, POS_THRESHOLD)
#print("-", VALUE, MIN, NEG_THRESHOLD)
if VALUE > 0:
_pos = MAX * POS_THRESHOLD * 0.01
if VALUE > _pos: # above positive threshold
VALUE = min((VALUE - _pos) / (MAX - _pos),
1.0) # normalize interval
else: # below positive threshold
VALUE = 0
elif VALUE < 0:
_neg = MIN * abs(NEG_THRESHOLD) * 0.01
if VALUE < _neg:
VALUE = max((VALUE - _neg) / abs(MIN - _neg), -1.0)
else: # above negative threshold
VALUE = 0
return VALUE
class MultiDofDevice(avango.script.Script):
# output fields
## @var mf_dof
# The input values of the input device.
mf_dof = avango.MFFloat()
mf_dof.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # init 8 channels
## @var mf_buttons
# The button values of the input device.
mf_buttons = avango.MFBool()
mf_buttons.value = [False, False, False, False, False, False, False] # init 7 buttons
## @var sf_station_mat
# Tracking position and rotation.
sf_station_mat = avango.gua.SFMatrix4()
sf_station_mat.value = avango.gua.make_identity_mat()
# factors for amplifying
## @var translation_factor
# Factor to modify the device's translation input.
translation_factor = 1.0
## @var rotation_factor
# Factor to modify the device's rotation input.
rotation_factor = 1.0
## Default constructor.
def __init__(self):
self.super(MultiDofDevice).__init__()
## Initializes a tracking reader for the device's position and rotation
# @param TRACKING_TARGET_NAME The tracking name as chosen in daemon, None if no tracking is available.
# @param NO_TRACKING_MAT Tracking matrix to be used if no tracking is available.
def init_station_tracking(self, TRACKING_TARGET_NAME, NO_TRACKING_MAT):
## @var tracking_reader
# Tracking Reader to process the tracking input of the device.
if TRACKING_TARGET_NAME != None:
self.tracking_reader = TrackingTargetReader()
self.tracking_reader.my_constructor(TRACKING_TARGET_NAME)
self.sf_station_mat.connect_from(self.tracking_reader.sf_abs_mat)
else:
self.tracking_reader = TrackingDefaultReader()
self.tracking_reader.set_no_tracking_matrix(NO_TRACKING_MAT)
self.sf_station_mat.connect_from(self.tracking_reader.sf_abs_mat)
## Map an input value to a certain interval.
# @param VALUE The value to be mapped.
# @param OFFSET The offset to be applied to VALUE, MIN and MAX
# @param MIN The minimum value of the new interval.
# @param MAX The maximum value of the old interval.
# @param NEG_THRESHOLD The negative threshold to be used.
# @param POS_THRESHOLD The positive threshold to be used.
def filter_channel(self, VALUE, OFFSET, MIN, MAX, NEG_THRESHOLD, POS_THRESHOLD):
VALUE = VALUE - OFFSET
MIN = MIN - OFFSET
MAX = MAX - OFFSET
#print "+", VALUE, MAX, POS_THRESHOLD
#print "-", VALUE, MIN, NEG_THRESHOLD
if VALUE > 0:
_pos = MAX * POS_THRESHOLD * 0.01
if VALUE > _pos: # above positive threshold
VALUE = min( (VALUE - _pos) / (MAX - _pos), 1.0) # normalize interval
else: # beneath positive threshold
VALUE = 0
elif VALUE < 0:
_neg = MIN * NEG_THRESHOLD * 0.01
if VALUE < _neg:
VALUE = max( (VALUE - _neg) / abs(MIN - _neg), -1.0)
else: # above negative threshold
VALUE = 0
return VALUE
class Manipulation(avango.script.Script):
### input fields
mf_dof = avango.MFFloat()
mf_dof.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # init 7 channels
mf_buttons = avango.MFBool()
mf_buttons.value = [False, False] # init 2 channels
### output_fields
sf_mat = avango.gua.SFMatrix4()
sf_mat.value = avango.gua.make_identity_mat()
sf_action_trigger = avango.SFBool()
### constructor
def __init__(self):
self.super(Manipulation).__init__()
### variables ###
self.type = ""
self.enable_flag = False
### callback functions ###
def evaluate(self): # evaluated every frame if any input field has changed
if self.enable_flag == True:
self.manipulate()
@field_has_changed(mf_buttons)
def mf_buttons_changed(self):
if self.enable_flag == True:
_left_button = self.mf_buttons.value[0]
_right_button = self.mf_buttons.value[1]
self.sf_action_trigger.value = _left_button ^ _right_button # button left XOR button right
### functions ###
def enable_manipulation(self, FLAG):
self.enable_flag = FLAG
if self.enable_flag == True:
print(self.type + " enabled")
self.reset()
def manipulate(self):
raise NotImplementedError("To be implemented by a subclass.")
def reset(self):
raise NotImplementedError("To be implemented by a subclass.")
def clamp_matrix(self, MATRIX):
# clamp translation to certain range (within screen space)
_x_range = 0.3 # in meter
_y_range = 0.15 # in meter
_z_range = 0.15 # in meter
MATRIX.set_element(0, 3,
min(_x_range,
max(-_x_range,
MATRIX.get_element(0, 3)))) # clamp x-axis
MATRIX.set_element(1, 3,
min(_y_range,
max(-_y_range,
MATRIX.get_element(1, 3)))) # clamp y-axis
MATRIX.set_element(2, 3,
min(_z_range,
max(-_z_range,
MATRIX.get_element(2, 3)))) # clamp z-axis
return MATRIX
class ManipulationManager(avango.script.Script):
### input fields
sf_key_1 = avango.SFFloat()
sf_key_2 = avango.SFFloat()
sf_key_3 = avango.SFFloat()
mf_dof = avango.MFFloat()
mf_buttons = avango.MFBool()
### internal fields
sf_hand_mat = avango.gua.SFMatrix4()
# constructor
def __init__(self):
self.super(ManipulationManager).__init__()
def my_constructor(self, PARENT_NODE, MOUSE_DEVICE, SPACEMOUSE_DEVICE):
# references
self.mouse_device = MOUSE_DEVICE
self.spacemouse_device = SPACEMOUSE_DEVICE
# variables
self.dragging_technique = None
self.dragged_objects = []
self.parent_node = PARENT_NODE
self.scene_root = PARENT_NODE
self.offset = avango.gua.make_identity_mat()
self.diff_mat = avango.gua.make_identity_mat()
self.last_frame = avango.gua.make_identity_mat()
while self.scene_root.Parent.value:
self.scene_root = self.scene_root.Parent.value
self.lf_hand_mat = avango.gua.make_identity_mat(
) # last frame hand matrix
### init hand geometry
_loader = avango.gua.nodes.TriMeshLoader(
) # init trimesh loader to load external meshes
self.hand_geometry = _loader.create_geometry_from_file(
"hand_geometry", "data/objects/hand.obj",
avango.gua.LoaderFlags.DEFAULTS)
self.hand_geometry.Transform.value = avango.gua.make_rot_mat(45.0,1,0,0) * \
avango.gua.make_rot_mat(180.0,0,1,0) * \
avango.gua.make_scale_mat(0.06)
self.hand_geometry.Material.value.set_uniform(
"Color", avango.gua.Vec4(1.0, 0.86, 0.54, 1.0))
self.hand_geometry.Material.value.set_uniform("Emissivity", 0.9)
self.hand_geometry.Material.value.set_uniform("Metalness", 0.1)
self.hand_transform = avango.gua.nodes.TransformNode(
Name="hand_transform")
self.hand_transform.Children.value = [self.hand_geometry]
self.hand_transform.Transform.value = avango.gua.make_identity_mat()
self.parent_node.Children.value.append(self.hand_transform)
self.sf_hand_mat.connect_from(self.hand_transform.WorldTransform)
# init manipulation technique
self.isotonic_position_control_manipulation = IsotonicPositionControlManipulation(
)
self.isotonic_position_control_manipulation.my_constructor(
self.hand_transform.Transform, self.mf_dof)
self.isotonic_position_control_manipulation.enable_manipulation(True)
self.mf_dof.connect_from(self.mouse_device.mf_dof)
self.mf_buttons.connect_from(self.mouse_device.mf_buttons)
# init dragging technique
self.set_dragging_technique(1)
# init keyboard sensor
self.keyboard_sensor = avango.daemon.nodes.DeviceSensor(
DeviceService=avango.daemon.DeviceService())
self.keyboard_sensor.Station.value = "device-keyboard"
self.sf_key_1.connect_from(self.keyboard_sensor.Button12) # key 1
self.sf_key_2.connect_from(self.keyboard_sensor.Button13) # key 2
self.sf_key_3.connect_from(self.keyboard_sensor.Button14) # key 3
### functions
def set_dragging_technique(self, INT):
self.dragging_technique = INT
print("Dragging Technique set to technique", self.dragging_technique)
def start_dragging(self):
_hand_mat = self.sf_hand_mat.value
# travers all scenegraph nodes
for _node in self.scene_root.Children.value:
_name = _node.Name.value
if _name.count("monkey") > 0: # a monkey node
if self.is_highlight_material(
_node.CurrentColor.value
): # monkey node in close proximity
_node.CurrentColor.value = avango.gua.Vec4(
1.0, 0.0, 0.0, 1.0)
_node.Material.value.set_uniform(
"Color", _node.CurrentColor.value
) # switch to dragging material
self.dragged_objects.append(_node) # add node for dragging
## dragging without snapping
## TODO: Implement dragging strategies here ##
if self.dragging_technique == 1: # change of node order in scenegraph
self.offset = avango.gua.make_inverse_mat(
self.hand_transform.WorldTransform.value
) * _node.Transform.value
self.scene_root.Children.value.remove(_node)
_node.Transform.value = self.offset
self.hand_transform.Children.value.append(_node)
elif self.dragging_technique == 2: # absolute tool-hand offset to tool space
self.offset = avango.gua.make_inverse_mat(
self.hand_transform.WorldTransform.value
) * _node.Transform.value
elif self.dragging_technique == 3: # relative tool input to object space
self.last_frame = self.hand_transform.WorldTransform.value
def object_dragging(self):
# apply hand movement to (all) dragged objects
for _node in self.dragged_objects:
## TODO: Implement dragging strategies here ##
if self.dragging_technique == 1: # change of node order in scenegraph
pass
elif self.dragging_technique == 2: # absolute tool-hand offset to tool space
_node.Transform.value = self.hand_transform.WorldTransform.value * self.offset
elif self.dragging_technique == 3: # relative tool input to object space
self.diff_mat = avango.gua.make_inverse_mat(
self.last_frame) * self.hand_transform.WorldTransform.value
self.last_frame = self.hand_transform.WorldTransform.value
_node.Transform.value = self.diff_mat * _node.Transform.value
def stop_dragging(self):
# travers all dragged objects
for _node in self.dragged_objects:
_node.CurrentColor.value = avango.gua.Vec4(0.0, 1.0, 0.0, 1.0)
_node.Material.value.set_uniform(
"Color",
_node.CurrentColor.value) # switch to highlight material
## TODO: Implement dragging strategies here ##
if self.dragging_technique == 1: # change of node order in scenegraph
_node.Transform.value = _node.WorldTransform.value
self.hand_transform.Children.value.remove(_node)
self.scene_root.Children.value.append(_node)
elif self.dragging_technique == 2: # absolute tool-hand offset to tool space
pass
elif self.dragging_technique == 3: # relative tool input to object space
pass
self.dragged_objects = [] # clear list
def is_default_material(self, VEC4):
return VEC4.x == 1.0 and VEC4.y == 1.0 and VEC4.z == 1.0 and VEC4.w == 1.0
def is_highlight_material(self, VEC4):
return VEC4.x == 0.0 and VEC4.y == 1.0 and VEC4.z == 0.0 and VEC4.w == 1.0
def is_dragging_material(self, VEC4):
return VEC4.x == 1.0 and VEC4.y == 0.0 and VEC4.z == 0.0 and VEC4.w == 1.0
### callbacks
@field_has_changed(sf_key_1)
def sf_key_1_changed(self):
if self.sf_key_1.value == True: # key is pressed
self.set_dragging_technique(1) # switch dragging technique
@field_has_changed(sf_key_2)
def sf_key_2_changed(self):
if self.sf_key_2.value == True: # key is pressed
self.set_dragging_technique(2) # switch dragging technique
@field_has_changed(sf_key_3)
def sf_key_3_changed(self):
if self.sf_key_3.value == True: # key is pressed
self.set_dragging_technique(3) # switch dragging technique
@field_has_changed(mf_buttons)
def mf_buttons_changed(self):
_left_button = self.mf_buttons.value[0]
_right_button = self.mf_buttons.value[1]
_button = _left_button ^ _right_button # button left XOR button right
if _button == True: # key is pressed
self.start_dragging()
else:
self.stop_dragging()
@field_has_changed(sf_hand_mat)
def sf_hand_mat_changed(self):
_hand_pos = self.sf_hand_mat.value.get_translate()
# travers all scenegraph nodes
for _node in self.scene_root.Children.value:
_name = _node.Name.value
if _name.count("monkey") > 0: # identify a monkey node
_pos = _node.Transform.value.get_translate(
) # a monkey position
_dist = (_hand_pos - _pos).length() # hand-object distance
_node_col = _node.CurrentColor.value
### toggle object highlight
if _dist < 0.02 and self.is_default_material(_node_col):
_node.CurrentColor.value = avango.gua.Vec4(
0.0, 1.0, 0.0, 1.0)
_node.Material.value.set_uniform(
"Color", _node.CurrentColor.value
) # switch to highlight material
elif _dist > 0.025 and self.is_highlight_material(_node_col):
_node.CurrentColor.value = avango.gua.Vec4(
1.0, 1.0, 1.0, 1.0)
_node.Material.value.set_uniform(
"Color",
_node.CurrentColor.value) # switch to default material
self.object_dragging() # evtl. drag object with hand input
# calc hand velocity
_distance = (_hand_pos - self.lf_hand_mat.get_translate()).length()
_velocity = _distance * 60.0 # application loop runs with 60Hz
_velocity = round(_velocity, 2) # round to 2nd decimal place
#print(_velocity, "m/s")
self.lf_hand_mat = self.sf_hand_mat.value
