Created
August 8, 2025 20:33
-
-
Save kwahoo2/6be97fd804ab3df0441eb0e710eafbd9 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| # moves selected shape along predefined path, and creates a 3D picking ray along object's Z axis | |
| # solid should be outside 0-Z+ to avoid self-picking | |
| # the ray picks another object in predefined location (waypoint changing from (plecement, False) to (placement, True)) and drags it | |
| from pivy.coin import SbVec3f, SbRotation | |
| from pivy.coin import SoTranslation | |
| from pivy.coin import SoSeparator, SoSwitch, SO_SWITCH_ALL, SO_SWITCH_NONE | |
| from pivy.coin import SoVertexProperty, SoLineSet, SoSphere | |
| from pivy.coin import SoBaseColor, SbColor | |
| import FreeCADGui as Gui | |
| import FreeCAD as App | |
| import UtilsAssembly | |
| from PySide6 import QtCore | |
| default_ray_len = 500 | |
| robot_tip = App.ActiveDocument.getObjectsByLabel('arm-tip')[0] # Adjust the object (label) for movement | |
| # the path will be created along these placements | |
| p0 = App.Placement(App.Vector(950,0,1269), App.Rotation(0,-70,0), App.Vector(0,0,0)) | |
| p1 = App.Placement(App.Vector(890,0,510), App.Rotation(0,0,0), App.Vector(0,0,0)) | |
| p2 = App.Placement(App.Vector(776,-630,1500), App.Rotation(0,-85,-45), App.Vector(0,0,0)) | |
| p3 = App.Placement(App.Vector(-40,-780,1573), App.Rotation(-85,-90,0), App.Vector(0,0,0)) | |
| p4 = App.Placement(App.Vector(-150,-800,1100), App.Rotation(-70,-30,0), App.Vector(0,0,0)) | |
| p5 = App.Placement(App.Vector(-200,-800,800), App.Rotation(-90,-0,0), App.Vector(0,0,0)) | |
| # path of the moving object (eg. robot arm): (placement, is dragging now) | |
| waypoints = ((p0, False), (p1, True), (p0, True), (p2, True), (p3, True), (p4, True), (p5, False), (p4, False), (p3, False), (p2, False)) | |
| class rayPicker: | |
| def __init__(self): | |
| sg = Gui.ActiveDocument.ActiveView.getSceneGraph() | |
| self.pos = SbVec3f() | |
| self.rot = SbRotation() | |
| self.tip_plac_at_sel = None # position when a object was picked | |
| self.curr_obj = None # selected(picked) object | |
| self.obj_plac_at_sel = None | |
| self.sel_pnt = None | |
| ray_sep = SoSeparator() | |
| self.ray_node = SoSwitch() | |
| self.ray_vtxs = SoVertexProperty() | |
| self.ray_vtxs.vertex.set1Value(0, 0, 0, 0) | |
| # set second vertex, later update to point of intersection ray with hit object | |
| self.ray_vtxs.vertex.set1Value(1, 0, 0, default_ray_len) | |
| ray_line = SoLineSet() | |
| ray_line.vertexProperty = self.ray_vtxs | |
| self.ray_color = SoBaseColor() | |
| self.ray_color.rgb = SbColor(1, 0, 0) | |
| ray_sep.addChild(self.ray_color) | |
| ray_sep.addChild(ray_line) | |
| self.sph_trans = SoTranslation() | |
| self.sph_trans.translation.setValue(0, 0, 0) | |
| self.sph_node = SoSwitch() | |
| self.sph_node.addChild(self.sph_trans) | |
| ray_sph = SoSphere() | |
| ray_sph.radius.setValue(20.0) | |
| self.sph_node.addChild(ray_sph) | |
| ray_sep.addChild(self.sph_node) | |
| self.ray_node.addChild(ray_sep) # SoSwitch behaves like SoNode, not SoSeparator | |
| sg.addChild(self.ray_node) | |
| def find_ray_axis(self, rot): | |
| ray_axis = rot.multVec(SbVec3f(0, 0, 1)) | |
| return ray_axis | |
| def show_ray(self): | |
| self.ray_node.whichChild = SO_SWITCH_ALL | |
| def hide_ray(self): | |
| self.ray_node.whichChild = SO_SWITCH_NONE | |
| def doc_to_coin_pnt(self, dpnt): | |
| if not dpnt: | |
| return None | |
| else: | |
| return SbVec3f(dpnt.x, dpnt.y, dpnt.z) | |
| def doc_to_coin_rot(self, drot): | |
| q = drot.Q | |
| rot = SbRotation(q[0], q[1], q[2], q[3]) | |
| return rot | |
| def coin_to_doc_pnt(self, cpnt): | |
| pnt = App.Vector( | |
| cpnt.getValue()[0], | |
| cpnt.getValue()[1], | |
| cpnt.getValue()[2]) | |
| return pnt | |
| # shows the ray (calculated elsewhere) | |
| # if there is no point it will show just ray, without sphere at the end | |
| def show_ray_ext(self, start_vec, rot, end_vec=None): | |
| self.ray_node.whichChild = SO_SWITCH_ALL | |
| ray_start_vec = start_vec | |
| ray_axis = self.find_ray_axis(rot) | |
| ray_end_vec = start_vec - ray_axis * default_ray_len | |
| if (end_vec): | |
| self.sph_node.whichChild = SO_SWITCH_ALL | |
| ray_end_vec = end_vec | |
| else: | |
| self.sph_node.whichChild = SO_SWITCH_NONE | |
| self.sph_trans.translation.setValue(ray_end_vec) | |
| self.ray_vtxs.vertex.set1Value(0, ray_start_vec) | |
| self.ray_vtxs.vertex.set1Value(1, ray_end_vec) | |
| return ray_end_vec | |
| def select_object(self, pl): | |
| doc = App.ActiveDocument | |
| start_vec = self.doc_to_coin_pnt(pl.Base) | |
| rot = self.doc_to_coin_rot(pl.Rotation) | |
| ray_axis = self.find_ray_axis(rot) | |
| vec_start = self.coin_to_doc_pnt(start_vec) | |
| vec_dir = self.coin_to_doc_pnt(-ray_axis) | |
| view = Gui.ActiveDocument.ActiveView | |
| # Document objects picking, Base::Vector is needed, not SbVec3f | |
| info = view.getObjectInfoRay(vec_start, vec_dir) | |
| if (info): | |
| sect_pt = info['PickedPoint'] | |
| self.curr_obj = doc.getObject(info['Object']) | |
| if 'ParentObject' in info: # check it a body should be moved instead | |
| parent = info['ParentObject'] | |
| p_obj = doc.getObject(parent) | |
| if hasattr(p_obj, 'Placement'): | |
| self.curr_obj = p_obj | |
| # support for objects belonging to assemblied omitted for simplicity | |
| print("Picked object: ", info) | |
| self.sel_pnt = sect_pt | |
| self.tip_plac_at_sel = pl | |
| if hasattr(self.curr_obj, 'Placement'): | |
| self.obj_plac_at_sel = self.curr_obj.Placement | |
| return sect_pt | |
| else: | |
| return None | |
| def drag_object(self, pl): | |
| if self.curr_obj: | |
| old_obj_plac = self.obj_plac_at_sel | |
| old_tip_plac = self.tip_plac_at_sel | |
| # calculating transformation between two robot tip poses | |
| plt_tip = pl * old_tip_plac.inverse() | |
| # calculate the selected obj new placement | |
| obj_plac = plt_tip * old_obj_plac | |
| self.curr_obj.Placement = obj_plac | |
| UtilsAssembly.activeAssembly().solve() | |
| # calculate the selection point new location | |
| s_pnt = plt_tip * self.sel_pnt | |
| return s_pnt | |
| else: | |
| return None | |
| def update_ray(self, action): | |
| pl = robot_tip.Placement | |
| if action: | |
| if action == 'pick': | |
| print ('Picking...') | |
| self.show_ray() | |
| pp = self.select_object(pl) | |
| picked_point = self.doc_to_coin_pnt(pp) | |
| start_vec = self.doc_to_coin_pnt(pl.Base) | |
| rot = self.doc_to_coin_rot(pl.Rotation) | |
| self.show_ray_ext(start_vec, rot, picked_point) | |
| elif action == 'drag': | |
| point = self.drag_object(pl) | |
| if point: | |
| self.show_ray() | |
| start_vec = self.doc_to_coin_pnt(pl.Base) | |
| rot = self.doc_to_coin_rot(pl.Rotation) | |
| self.show_ray_ext(start_vec, rot, point) | |
| else: | |
| self.hide_ray() | |
| elif action == 'release': | |
| print ('Releasing...') | |
| self.hide_ray() | |
| self.curr_obj = None | |
| else: | |
| self.hide_ray() | |
| class Animation(object): | |
| def __init__(self): | |
| App.ActiveDocument.recompute() | |
| self.timer = QtCore.QTimer() | |
| QtCore.QObject.connect(self.timer, QtCore.SIGNAL("timeout()"), self.my_update) | |
| self.timer.start(50) | |
| self.curr_wp = 0 | |
| self.next_wp = 1 | |
| self.t = 0 | |
| self.t_step = 0.05 | |
| self.drag = False | |
| self.action = None | |
| self.rp = rayPicker() | |
| def my_update(self): | |
| # path is repating in circular pattern | |
| if (self.t > 1) and (self.next_wp > (len(waypoints) - 3)): | |
| self.next_wp = 0 | |
| self.curr_wp = self.curr_wp + 1 | |
| self.t = 0 | |
| elif (self.t > 1) and (self.curr_wp > (len(waypoints) - 3)): | |
| self.curr_wp = 0 | |
| self.next_wp = self.next_wp + 1 | |
| self.t = 0 | |
| elif self.t > 1: | |
| self.curr_wp = self.curr_wp + 1 | |
| self.next_wp = self.next_wp + 1 | |
| self.t = 0 | |
| wp = waypoints[self.curr_wp] | |
| wp1 = waypoints[self.next_wp] | |
| pl = wp[0] | |
| pl1 = wp1[0] | |
| robot_tip.Placement = pl.sclerp(pl1, self.t) | |
| UtilsAssembly.activeAssembly().solve() | |
| if self.t == 0: # check only on the starting point | |
| if wp[1] and not self.drag: | |
| self.drag = True | |
| self.action = 'pick' | |
| elif not wp[1] and self.drag: | |
| self.drag = False | |
| self.action = 'release' | |
| else: | |
| if self.drag: | |
| self.action = 'drag' | |
| else: | |
| self.action = None | |
| self.rp.update_ray(self.action) | |
| self.t = self.t + self.t_step | |
| def stop(self): | |
| self.timer.stop() | |
| animation = Animation() | |
| # To stop the animation, type: | |
| # animation.stop() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment