[Albert]

It looks like you already have a 4x4 matrix in the text file you provided.

You can create this matrix (Mat) by directly providing the all components of your matrix using a list of lists in Python:

Code:

poselist = [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]
pose = robomath.Mat(poselist)
if not pose.isHomogeneous():
print("Something is wrong: this pose is not homogeneous.")
print(pose)

The opposite of Pose_2_ABB would be a function that takes 7 values (xyz,q1,q2,q3,q4) and returns a pose (4x4 matrix). This function could look like this:

Code:

def ABB_2_Pose(xyzq1234):
xyz = xyzq1234[0:3]
q1234 = xyzq1234[3:7]
pose = quaternion_2_pose(q1234)
pose.setPos(xyz)
return pose