Robot WorkBench - Orientation

[OsWeldo]

Hey guys, I'm having some problems using Robot Workbench.
So I want the wrist to be perpendicular to the faces, I have calculated the quaternions as you can see below.

Code: Select all

    def Orientacao_Pontos(self,Pontos_Trajetoria):
       orientacao =[]      
       for point in Pontos_Trajetoria:
            ponto = ponto.split("\t")

            for face in self.Faces.values():
                if face.BoundBox.XMin <= float(ponto[0]) <= face.BoundBox.XMax:
                    Superficie = face.Surface
                    uv = Superficie.parameter(Vector(float(ponto[0]), float(ponto[1]), float(ponto[2])))
                    vetor = face.normalAt(uv[0], uv[1])
                    vetor = vetor.normalize()
                    xyz=sin(vetor.getAngle(Vector(0,0,1))/2)*(Vector(0,0,1))/(Vector(0,0,1).Length)
                    w = cos(vetor.getAngle(Vector(0,0,1)) / 2)
                    quaternion= (xyz[0],xyz[1],xyz[2],w)
                    orientacao.append(quaternion)

And inserted the waypoints this way:

Code: Select all

    def Trajetoria_Robo(self,Pontos_Trajetoria,orientacao):

        FreeCAD.activeDocument().addObject("Robot::TrajectoryObject", "Trajectory")
        t = FreeCAD.activeDocument().Trajectory.Trajectory
        for i in range(0,len(Pontos_Trajetoria)):
            ponto = Vector(Pontos_Trajetoria[i].split("\t"))
            orientacao_ponto = FreeCAD.Rotation(orientacao[i][0],orientacao[i][1],orientacao[i][2],orientacao[i][3])
            print(orientacao_ponto)
            t.insertWaypoints(Waypoint(Placement(ponto, orientacao_ponto), "LIN", "Pt"))
        FreeCAD.activeDocument().Trajectory.Trajectory = t

But what happens is that the pulse is not the way it should be as you can see in the figure.

OS: Windows 10 Version 2004
Word size of OS: 64-bit
Word size of FreeCAD: 64-bit
Version: 0.19.24276 (Git)
Build type: Release
Branch: releases/FreeCAD-0-19
Hash: a88db11e0a908f6e38f92bfc5187b13ebe470438
Python version: 3.8.6+
Qt version: 5.15.1
Coin version: 4.0.1
OCC version: 7.5.0
Locale: Portuguese/Portugal (pt_PT)

[Kunda1]

It's cool to see robot wb mentioned but JFYI it hasn't been maintained or had an active maintainer in several years.

[OsWeldo]

Yeah, unfortunately... But it suits for what i'm looking for, just having this little issue defining the orientation and to define the TCP.