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