Allow use of radians or degrees for angle units in poses

[sea-bass]

Suggested by a user: "Another annoyance is the use of radians for pose angles. Degrees would be much
more user friendly for text formats."

This would involve making some slight changes to the Pose constructor and/or static methods to specify the units.

I would maintain that radians should be the default, though.

[kumar-sanjeeev]

Hello @sea-bass , I looked into this issue and worked on a solution. I haven’t fully tested it yet but will make sure to do so before submitting a PR.

My Idea:

I added a new class argument angle_units, which defaults to radians but can also be set to degrees for defining a Pose. I updated the default constructor and other methods like from_list() and from_dict() to use this.

Tested:

I tested this, with spawning table in demo script.

Here’s a snippet of the default constructor and demo:

#########
 Default Constructor
#########
class Pose:
    """Represents a 3D pose."""

    def __init__(
        self,
        x=0.0,
        y=0.0,
        z=0.0,
        roll=0.0,
        pitch=0.0,
        yaw=0.0,
        q=None,
        angle_units="radians",
    ):
        """
        Creates a new Pose object.

        :param x: X position
        :type x: float
        :param y: Y position
        :type y: float
        :param z: Z position
        :type z: float
        :param roll: Roll angle (about X axis), in radians
        :type roll: float
        :param pitch: Pitch angle (about Y axis), in radians
        :type pitch: float
        :param yaw: Yaw angle (about Z axis), in radians
        :type yaw: float
        :param q: Quaternion, specified at [qw, qx, qy, qz]
            If specified, will override roll/pitch/yaw values.
        :type q: list[float]
        :param angle_units: Units for angle ('radians' or `degrees`)
        :type angle_units: str
        """
        self.x = x
        self.y = y
        self.z = z

        if angle_units == "degrees":
            roll, pitch, yaw = map(math.radians, [roll, pitch, yaw])

        if q is not None:
            self.set_quaternion(q)
        else:
            self.set_euler_angles(roll, pitch, yaw)
#########

#########
 Spawning table code from demo
#########

    # Add locations
    table = world.add_location(
        category="table",
        parent="kitchen",
        pose=Pose(x=0.85, y=-0.5, z=0.0, yaw=-90, angle_units="degrees"),
    )
#########

Let me know your thoughts on this approach. If there’s anything I missed (any pitfall) or could improve, please let me know.

[sea-bass]

That looks in line with what I was thinking!

Just know that there a bunch of static methods in the Pose class that also need to be given this angle unit.

The yaml_utils.py file, and all the calls to Pose.construct() may need to be looked at here.

As before, the true test is whether putting the angle units in the world YAML file works, and the world can then be reset from the GUI.

[kumar-sanjeeev]

I noted your points. When you mentioned static methods, did you mean class methods in the Pose class? I will review yaml_utils.py and make sure all constructors in the Pose class work correctly for both radians and degrees.

For testing, do we need to create new tests for this change, or are there already existing tests that I can run to check if anything breaks with the new functionality?

[sea-bass]

Probably yes, there should be some good tests already for the Pose class, but they need to additionally stress test degrees vs radians vs invalid units.

And once we have this in place, I think it makes sense to update all the example worlds to use degrees.

As the original issue noted, the main use case was it's awkward to express multiples of pi/2 in radians but it's nice round numbers in degrees.

[kumar-sanjeeev]

Okay, understood. I think I have enough inputs to start working on this issue. If I need to discuss anything while working on it, I’ll reach out.