crank-animation-accel.py

import FreeCAD as App, FreeCADGui as Gui, Part, time
from PySide2 import QtGui,QtCore

from pivy import coin
import math

class CustomArrow(object):
    def __init__(self):
        self.cyl_height = 6
        cyl_radius = 3
        self.cone_height = 4
        cone_radius = 6
        sg = Gui.ActiveDocument.ActiveView.getSceneGraph()
        self.col = coin.SoBaseColor()
        self.col.rgb = (1, 0, 0)
        cyl = coin.SoCylinder()
        cone = coin.SoCone()

        self.scale = coin.SoScale()
        self.trans = coin.SoTranslation()
        self.trans.translation.setValue([0, 0, 0])
        self.cyl_trans = coin.SoTranslation()
        self.cyl_trans.translation.setValue([0, -self.cyl_height/2, 0])
        cone_trans = coin.SoTranslation()
        cone_trans.translation.setValue([0, -self.cyl_height/2 -self.cone_height/2, 0])
        cone_rot = coin.SoRotationXYZ()
        cone_rot.axis.setValue(coin.SoRotationXYZ.X)
        cone_rot.angle.setValue(-math.pi)

        cyl.height.setValue(self.cyl_height)
        cyl.radius.setValue(cyl_radius)
        cone.height.setValue(self.cone_height)
        cone.bottomRadius.setValue(cone_radius)

        self.rot = coin.SoRotationXYZ() # rotate scene to set Z as vertical
        self.rot.axis.setValue(coin.SoRotationXYZ.X)
        self.rot.angle.setValue(-math.pi/2)

        arrow_node = coin.SoSeparator()
        arrow_node.addChild(self.col)
        arrow_node.addChild(self.trans)
        arrow_node.addChild(self.rot)
        arrow_node.addChild(self.scale)
        arrow_node.addChild(self.cyl_trans)
        arrow_node.addChild(cyl)
        arrow_node.addChild(cone_trans)
        arrow_node.addChild(cone_rot)
        arrow_node.addChild(cone)

        sg.addChild(arrow_node)

    def adjust_arrow(self, val, plac):
        if val < 0:
            self.rot.angle.setValue(math.pi/2)
            self.cyl_trans.translation.setValue([0, self.cyl_height/2 + self.cone_height, 0])
        else:
            self.rot.angle.setValue(-math.pi/2)
            self.cyl_trans.translation.setValue([0, -self.cyl_height/2, 0])
        self.scale.scaleFactor.setValue([1,abs(val),1])
        self.trans.translation.setValue(plac)

    def set_color(self, color):
        self.col.rgb = color

class Piston(object):
    def __init__(self, obj):
        App.Console.PrintMessage('calc obj created for: ' + str(obj.Label) + "\n")
        self.obj = obj
        self.iteration = 0
        self.old_position = obj.Placement.Base.z / 1000
        self.old_velocity = None

    def calc_accel(self, dt):
        obj = self.obj
        position = obj.Placement.Base.z / 1000 # mm to meters
        translation = position - self.old_position
        velocity = translation / dt
        acceleration = None
        if self.iteration > 0:
            acceleration = (velocity - self.old_velocity) / dt
        self.old_position = position
        self.old_velocity =  velocity
        App.Console.PrintMessage(str(obj.Label) + " Pos: " + str(position * 1000) + " Vel: " + str(velocity) + " Accel: " + str(acceleration) + "\n")
        self.iteration = self.iteration + 1
        return acceleration


class Animation(object):
    def __init__(self):
        App.Console.PrintMessage("init \n")

        App.ActiveDocument.recompute()

        self.timer = QtCore.QTimer()
        QtCore.QObject.connect(self.timer, QtCore.SIGNAL("timeout()"), self.my_update)
        self.timer.start(100) #100ms for step, adjust to performance of the PC

        self.dt = 1.0 / 36000.0 # 360 deg * 6000 rpm / 60s/m
        self.an = 1
        self.p0 = App.ActiveDocument.getObjectsByLabel('piston')[0]
        self.p1 = App.ActiveDocument.getObjectsByLabel('piston001')[0]
        self.piston0 = Piston(self.p0)
        self.piston1 = Piston(self.p1)
        self.arrow0 = CustomArrow()
        self.arrow0.set_color((1, 0, 0))
        self.arrow1 = CustomArrow()
        self.arrow1.set_color((0, 1, 0))

        self.prb0 = App.ActiveDocument.getObjectsByLabel('probe')[0] # probe for first order inbalances
        self.prb1 = App.ActiveDocument.getObjectsByLabel('probe001')[0]
        self.probe0 = Piston(self.prb0)
        self.probe1 = Piston(self.prb1)
        self.arrow_diff0 = CustomArrow()
        self.arrow_diff0.set_color((0, 0, 1))
        self.arrow_diff1 = CustomArrow()
        self.arrow_diff1.set_color((0, 0, 1))
        self.arrow_total = CustomArrow()
        self.arrow_total.set_color((1, 1, 1))

    def my_update(self):
        self.an = self.an + 1
        App.ActiveDocument.getObject('Fixed').Rotation = self.an
        # App.ActiveDocument.recompute()
        Gui.runCommand('Assembly_SolveAssembly',0)
        accel0 = self.piston0.calc_accel(self.dt)
        if accel0 is not None:
            plac0 = self.p0.Placement.Base
            self.arrow0.adjust_arrow(accel0/1000, plac0)

        accel1 = self.piston1.calc_accel(self.dt)
        if accel1 is not None:
            plac1 = self.p1.Placement.Base
            self.arrow1.adjust_arrow(accel1/1000, plac1)
        accel_probe0 = self.probe0.calc_accel(self.dt)
        accel_probe1 = self.probe1.calc_accel(self.dt)
        if accel_probe0 is not None:
            self.arrow_diff0.adjust_arrow((accel0 - accel_probe0)/1000, coin.SbVec3f(plac0.x + 10, plac0.y, plac0.z))
        if accel_probe1 is not None:
            self.arrow_diff1.adjust_arrow((accel1 - accel_probe1)/1000, coin.SbVec3f(plac1.x + 10, plac1.y, plac1.z))
            self.arrow_total.adjust_arrow(((accel1 - accel_probe1) + (accel0 - accel_probe0))/1000, coin.SbVec3f(0, 0, 0))
        #Uncomment line below to save pics
        #Gui.ActiveDocument.ActiveView.saveImage('/home/adi/freecad-projekty/mortargear/' +str(self.an)+'.png',1280,720,'Current')
        print(str(self.an) + "\n")

    def stop(self):
        self.timer.stop()


animation = Animation()
# type animation.stop() to stop