i encountered problem using python function "point_Zaxis_2_pose" when a certain angle is 0 or pi, in fact i think that in your original code there is not a well managing of this case:

The console returns an error in the function " xaxis = normalize3(cross(yaprox, zaxis)) " where the function cross is 0, and the the "normalize3" function has an operation of 1/norm(a) that is a number divided by zero.

The problems lies in the "if" condition that does not manage this case: in fact i do not understand why the condition to change the vector (yaprox) with which make the "cross"function is " if angle3(zaxis, yaprox) < 2 * pi / 180", as the operation " 2 * pi / 180" is only for changing the unity measure from degrees to radians, but the function "angle3" returns only radians.

I changed the function as below using a different condition and it works correctly:

Do you think there is a bug in the original code?

Code:

`def point_Zaxis_2_pose(point, zaxis, yaxis_hint1=[0, 0, 1], yaxis_hint2=[0, 1, 1]):`

"""Returns a pose given the origin as a point, a Z axis and a preferred orientation for the Y axis"""

pose = eye(4)

pose.setPos(point)

pose.setVZ(zaxis)

yaprox = yaxis_hint1

if angle3(zaxis, yaprox) < 2 * pi / 180:

yaprox = yaxis_hint2

xaxis = normalize3(cross(yaprox, zaxis))

yaxis = cross(zaxis, xaxis)

pose.setVX(xaxis)

pose.setVY(yaxis)

return pose

The console returns an error in the function " xaxis = normalize3(cross(yaprox, zaxis)) " where the function cross is 0, and the the "normalize3" function has an operation of 1/norm(a) that is a number divided by zero.

The problems lies in the "if" condition that does not manage this case: in fact i do not understand why the condition to change the vector (yaprox) with which make the "cross"function is " if angle3(zaxis, yaprox) < 2 * pi / 180", as the operation " 2 * pi / 180" is only for changing the unity measure from degrees to radians, but the function "angle3" returns only radians.

I changed the function as below using a different condition and it works correctly:

Code:

`def point_Zaxis_2_poseRAMTIN(point, zaxis, yaxis_hint1=[0, 0, 1], yaxis_hint2=[0, 1, 1]):`

"""Returns a pose given the origin as a point, a Z axis and a preferred orientation for the Y axis"""

pose = robomath.eye(4)

pose.setPos(point)

pose.setVZ(zaxis)

yaprox = yaxis_hint1

if ((robomath.angle3(zaxis, yaprox) <= 0.05 ) or((robomath.angle3(zaxis, yaprox) >= math.pi -0.05))):

yaprox = yaxis_hint2

xaxis = robomath.normalize3(robomath.cross(yaprox, zaxis))

yaxis = robomath.cross(zaxis, xaxis)

pose.setVX(xaxis)

pose.setVY(yaxis)

return pose

Do you think there is a bug in the original code?