URDF creation uses current joint values as new zero

[mrudorfer]

Hi. :)

Describe the Bug
Exporting a robot (proto) to URDF using the Robot.getUrdf() function uses the current joint configuration of the robot. I.e., creating the URDF in different joint configurations leads to different values for rpy in the origin tag of the joints.
There might be use cases in which this behaviour is desired, however, I did not find it to be documented and I did not expect this behaviour. In my case, I exported the URDF to use a library such as kinpy or ikpy for forward/inverse kinematics and the results did not match as the URDF was using different joint angles than the robot in Webots.

Steps to Reproduce
Create new world, add UR5e. Assign the following controller:

from controller import Robot
import numpy as np

TIME_STEP = 32

class UR5e(Robot):
    def __init__(self):
        super().__init__()
        
        self.motors = [
            self.getDevice("shoulder_pan_joint"),
            self.getDevice("shoulder_lift_joint"),
            self.getDevice("elbow_joint"),
            self.getDevice("wrist_1_joint"),
            self.getDevice("wrist_2_joint"),
            self.getDevice("wrist_3_joint"),
        ]
        for m in self.motors:
            m.getPositionSensor().enable(TIME_STEP)
            
        print('robot initialised.')
        
    def create_urdf(self, urdf_fn='../../resources/ur5e.urdf'):
        with open(urdf_fn, "w") as file:
            file.write(self.getUrdf())
        print(f'saved urdf to {urdf_fn} with joint pos {self.joint_pos()}')

    def joint_pos(self):
        return np.asarray([m.getPositionSensor().getValue() for m in self.motors])
        
    def move_to_joint_pos(self, target_joint_pos, timeout=2):
        for pos, motor in zip(target_joint_pos, self.motors):
            motor.setPosition(pos)
            
        #step through simulation using MAX_STEPS
        for step in range(timeout * 1000 // TIME_STEP):
            self.step()

if __name__ == '__main__':
    robot = UR5e()
    robot.create_urdf('../../resources/ur5e_1.urdf')
    robot.move_to_joint_pos([0, -1.57, 1.57, -1.57, -1.57, 0.0])
    robot.create_urdf('../../resources/ur5e_2.urdf')

Run simulation, output should be:

robot initialised.
saved urdf to ../../resources/ur5e_1.urdf with joint pos [0. 0. 0. 0. 0. 0.]
saved urdf to ../../resources/ur5e_2.urdf with joint pos [-3.64427135e-05 -1.56998247e+00  1.57001749e+00 -1.56999837e+00
 -1.57000000e+00  4.76595666e-10]

Use pybullet to visualise robot with all joints set to zero based on the two URDF files:

import pybullet as p
import pybullet_data
import numpy as np

PATH_TO_URDF = 'resources/ur5e_2.urdf'

class Robot():
    def __init__(self, urdf_fn):
        p.connect(p.GUI)
        p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
        p.setAdditionalSearchPath(pybullet_data.getDataPath())
        p.loadURDF('plane.urdf')
        
        self.robot_id = p.loadURDF(urdf_fn, useFixedBase=True)
        self.joint_ids = list(range(6))
        
    def joint_pos(self):
        joint_states = p.getJointStates(self.robot_id, self.joint_ids)
        pos = [joint_states[joint][0] for joint in self.joint_ids]
        return np.array(pos)
        
    def reset_joints(self, joint_pos):
        for joint_id, q in zip(self.joint_ids, joint_pos):
            p.resetJointState(self.robot_id, joint_id, q)

def main():
    robot = Robot(PATH_TO_URDF)
    robot.reset_joints([0] * 6)
    print('pos:', robot.joint_pos())
    input('press any key')
    p.disconnect()

if __name__ == '__main__':
	main()

See difference between ur5e_1.urdf and ur5e_2.urdf:

Expected behavior
Multiple options:

• Irrespective of current joint configuration, the generated URDF should always be the same, assuming all joint positions to be 0.
• Alternatively, there could be a flag that allows to switch between current joint values and zero configuration.
• If the current behaviour is desired due to other reasons that I don't see now, it should be clearly described in the documentation. A warning could be printed whenever a non-zero configuration is used for saving the URDF.

System
Windows 10, Webots 2023b