Thread Rating:
  • 0 Vote(s) - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5

point_Zaxis_2_pose not managing 0 and pi angles

#1
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:

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?
#2
The point_Zaxis_2_pose function, available in robomath, was updated earlier this year to fix this issue. The update is available on PyPI, starting v5.5.5.

You find the latest version of the code here:
https://github.com/RoboDK/RoboDK-API/blo...th.py#L265

You can update robodk to the latest using:
Code:
python -m pip install --upgrade robodk

We also added point_Xaxis_2_pose and point_Yaxis_2_pose, more information in the Python documentation:
https://robodk.com/doc/en/PythonAPI/robo...xis_2_pose
Please read the Forum Guidelines before posting!
Find useful information about RoboDK by visiting our Online Documentation.
  




Users browsing this thread:
3 Guest(s)