diff --git a/tests/expected/Human.proto b/tests/expected/Human.proto index dd72866..1b8725d 100644 --- a/tests/expected/Human.proto +++ b/tests/expected/Human.proto @@ -1,4 +1,4 @@ -#VRML_SIM R2023a utf8 +#VRML_SIM R2023b utf8 # license: Apache License 2.0 # license url: http://www.apache.org/licenses/LICENSE-2.0 # This is a proto file for Webots for the Human @@ -362,14 +362,8 @@ PROTO Human [ } ] name "calcn_r" - boundingObject Transform { - scale 1.000000 -1.000000 1.000000 - children [ - DEF Calcn_L_001_cw Mesh { - url "../sources/gait2392_simbody/meshes/obj/Calcn L.001.obj" - ccw FALSE - } - ] + boundingObject DEF Calcn_R_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Calcn R.001.obj" } physics Physics { density -1 diff --git a/tests/expected/HumanR2022a.proto b/tests/expected/HumanR2022a.proto index 08c967c..7cc401c 100644 --- a/tests/expected/HumanR2022a.proto +++ b/tests/expected/HumanR2022a.proto @@ -439,14 +439,8 @@ PROTO HumanR2022a [ } ] name "calcn_r" - boundingObject Transform { - scale 1.000000 -1.000000 1.000000 - children [ - DEF Calcn_L_001_cw Mesh { - url "../sources/gait2392_simbody/meshes/obj/Calcn L.001.obj" - ccw FALSE - } - ] + boundingObject DEF Calcn_R_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Calcn R.001.obj" } physics Physics { density -1 diff --git a/tests/expected/HumanR2023a.proto b/tests/expected/HumanR2023a.proto new file mode 100644 index 0000000..c5cbcac --- /dev/null +++ b/tests/expected/HumanR2023a.proto @@ -0,0 +1,793 @@ +#VRML_SIM R2023a utf8 +# license: Apache License 2.0 +# license url: http://www.apache.org/licenses/LICENSE-2.0 +# This is a proto file for Webots for the HumanR2023a +# Extracted from: ../sources/gait2392_simbody/urdf/human.urdf + +PROTO HumanR2023a [ + field SFVec3f translation 0 0 0 + field SFRotation rotation 0 0 1 0 + field SFString name "HumanR2023a" # Is `Robot.name`. + field SFString controller "void" # Is `Robot.controller`. + field MFString controllerArgs [] # Is `Robot.controllerArgs`. + field SFString customData "" # Is `Robot.customData`. + field SFBool supervisor FALSE # Is `Robot.supervisor`. + field SFBool synchronization TRUE # Is `Robot.synchronization`. + field SFBool selfCollision FALSE # Is `Robot.selfCollision`. +] +{ + Robot { + translation IS translation + rotation IS rotation + controller IS controller + controllerArgs IS controllerArgs + customData IS customData + supervisor IS supervisor + synchronization IS synchronization + selfCollision IS selfCollision + children [ + DEF Pelvis_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Pelvis.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 1.000000 0.000000 + anchor -0.070700 0.083500 -0.066100 + } + device [ + RotationalMotor { + name "joint_femur_l" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_femur_l_sensor" + } + ] + endPoint Solid { + translation -0.070700 0.083500 -0.066100 + children [ + DEF Femur_L_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Femur L.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 1.000000 0.000000 + anchor -0.004500 0.000000 -0.395821 + } + device [ + RotationalMotor { + name "joint_tibia_l" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_tibia_l_sensor" + } + ] + endPoint Solid { + translation -0.004500 0.000000 -0.395821 + children [ + DEF Tibia_L_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Tibia L.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 1.000000 0.000000 + anchor 0.000000 0.000000 -0.430000 + } + device [ + RotationalMotor { + name "joint_talus_l" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_talus_l_sensor" + } + ] + endPoint Solid { + translation 0.000000 0.000000 -0.430000 + children [ + DEF Talus_L_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Talus L.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 1.000000 0.000000 + anchor -0.048770 0.007920 -0.041950 + } + device [ + RotationalMotor { + name "joint_calcn_l" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_calcn_l_sensor" + } + ] + endPoint Solid { + translation -0.048770 0.007920 -0.041950 + children [ + DEF Calcn_L_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Calcn L.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 1.000000 0.000000 + anchor 0.178800 0.001080 -0.002000 + } + device [ + RotationalMotor { + name "joint_toes_l" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_toes_l_sensor" + } + ] + endPoint Solid { + translation 0.178800 0.001080 -0.002000 + children [ + DEF Toes_L_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Toes L.obj" + } + ] + name "toes_l" + boundingObject DEF Toes_L_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Toes L.001.obj" + } + physics Physics { + density -1 + mass 0.216600 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 1.000000e-04 2.000000e-04 1.000000e-03 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "calcn_l" + boundingObject DEF Calcn_L_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Calcn L.001.obj" + } + physics Physics { + density -1 + mass 1.250000 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 1.400000e-03 3.900000e-03 4.100000e-03 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "talus_l" + boundingObject DEF Talus_L_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Talus L.001.obj" + } + physics Physics { + density -1 + mass 0.100000 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 1.000000e-03 1.000000e-03 1.000000e-03 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "tibia_l" + boundingObject DEF Tibia_L_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Tibia L.001.obj" + } + physics Physics { + density -1 + mass 3.707500 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 5.040000e-02 5.100000e-03 5.110000e-02 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "femur_l" + boundingObject DEF Femur_L_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Femur L.001.obj" + } + physics Physics { + density -1 + mass 9.301400 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 1.339000e-01 3.510000e-02 1.412000e-01 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + HingeJoint { + jointParameters HingeJointParameters { + anchor -0.070700 -0.083500 -0.066100 + } + device [ + RotationalMotor { + name "joint_femur_r" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_femur_r_sensor" + } + ] + endPoint Solid { + translation -0.070700 -0.083500 -0.066100 + children [ + DEF Femur_R_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Femur R.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 -1.000000 0.000000 + anchor -0.004500 0.000000 -0.395821 + } + device [ + RotationalMotor { + name "joint_tibia_r" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_tibia_r_sensor" + } + ] + endPoint Solid { + translation -0.004500 0.000000 -0.395821 + children [ + DEF Tibia_R_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Tibia R.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 -1.000000 0.000000 + anchor 0.000000 0.000000 -0.430000 + } + device [ + RotationalMotor { + name "joint_talus_r" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_talus_r_sensor" + } + ] + endPoint Solid { + translation 0.000000 0.000000 -0.430000 + children [ + DEF Talus_R_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Talus R.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 -1.000000 0.000000 + anchor -0.048770 -0.007920 -0.041950 + } + device [ + RotationalMotor { + name "joint_calcn_r" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_calcn_r_sensor" + } + ] + endPoint Solid { + translation -0.048770 -0.007920 -0.041950 + children [ + Transform { + scale 1.000000 -1.000000 1.000000 + children [ + DEF Calcn_L_visual_cw CadShape { + url "../sources/gait2392_simbody/meshes/obj/Calcn L.obj" + ccw FALSE + } + ] + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 -1.000000 0.000000 + anchor 0.178800 -0.001080 -0.002000 + } + device [ + RotationalMotor { + name "joint_toes_r" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_toes_r_sensor" + } + ] + endPoint Solid { + translation 0.178800 -0.001080 -0.002000 + children [ + DEF Toes_R_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Toes R.obj" + } + ] + name "toes_r" + boundingObject DEF Toes_R_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Toes R.001.obj" + } + physics Physics { + density -1 + mass 0.216600 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 1.000000e-04 2.000000e-04 1.000000e-03 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "calcn_r" + boundingObject DEF Calcn_R_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Calcn R.001.obj" + } + physics Physics { + density -1 + mass 1.250000 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 1.400000e-03 3.900000e-03 4.100000e-03 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "talus_r" + boundingObject DEF Talus_R_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Talus R.001.obj" + } + physics Physics { + density -1 + mass 0.100000 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 1.000000e-03 1.000000e-03 1.000000e-03 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "tibia_r" + boundingObject DEF Tibia_R_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Tibia R.001.obj" + } + physics Physics { + density -1 + mass 3.707500 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 5.040000e-02 5.100000e-03 5.110000e-02 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "femur_r" + boundingObject DEF Femur_R_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Femur R.001.obj" + } + physics Physics { + density -1 + mass 9.301400 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 1.339000e-01 3.510000e-02 1.412000e-01 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 1.000000 0.000000 + anchor -0.100700 0.000000 0.081500 + } + device [ + RotationalMotor { + name "joint_torso" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_torso_sensor" + } + ] + endPoint Solid { + translation -0.100700 0.000000 0.081500 + children [ + DEF Torso_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Torso.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 1.000000 0.000000 + anchor 0.003155 0.170000 0.371500 + } + device [ + RotationalMotor { + name "joint_humerus_l" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_humerus_l_sensor" + } + ] + endPoint Solid { + translation 0.003155 0.170000 0.371500 + children [ + DEF Humerus_L_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Humerus L.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 1.000000 0.000000 + anchor 0.013144 -0.009595 -0.286273 + } + device [ + RotationalMotor { + name "joint_ulna_l" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_ulna_l_sensor" + } + ] + endPoint Solid { + translation 0.013144 -0.009595 -0.286273 + children [ + DEF Ulna_L_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Ulna L.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 1.000000 0.000000 + anchor -0.006727 0.026083 -0.013007 + } + device [ + RotationalMotor { + name "joint_radius_l" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_radius_l_sensor" + } + ] + endPoint Solid { + translation -0.006727 0.026083 -0.013007 + children [ + DEF Radius_L_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Radius L.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 1.000000 0.000000 + anchor -0.008797 0.013610 -0.235841 + } + device [ + RotationalMotor { + name "joint_hand_l" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_hand_l_sensor" + } + ] + endPoint Solid { + translation -0.008797 0.013610 -0.235841 + children [ + DEF Hand_L_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Hand L.obj" + } + ] + name "hand_l" + boundingObject DEF Hand_L_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Hand L.001.obj" + } + physics Physics { + density -1 + mass 0.457500 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 8.920000e-04 5.470000e-04 1.340000e-03 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "radius_l" + boundingObject DEF Radius_L_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Radius L.001.obj" + } + physics Physics { + density -1 + mass 0.607500 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 2.962000e-03 6.180000e-04 3.213000e-03 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "ulna_l" + boundingObject DEF Ulna_L_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Ulna L.001.obj" + } + physics Physics { + density -1 + mass 0.607500 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 2.962000e-03 6.180000e-04 3.213000e-03 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "humerus_l" + boundingObject DEF Humerus_L_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Humerus L.001.obj" + } + physics Physics { + density -1 + mass 2.032500 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 1.194600e-02 4.121000e-03 1.340900e-02 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 -1.000000 0.000000 + anchor 0.003155 -0.170000 0.371500 + } + device [ + RotationalMotor { + name "joint_humerus_r" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_humerus_r_sensor" + } + ] + endPoint Solid { + translation 0.003155 -0.170000 0.371500 + children [ + DEF Humerus_R_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Humerus R.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 -1.000000 0.000000 + anchor 0.013144 0.009595 -0.286273 + } + device [ + RotationalMotor { + name "joint_ulna_r" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_ulna_r_sensor" + } + ] + endPoint Solid { + translation 0.013144 0.009595 -0.286273 + children [ + DEF Ulna_R_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Ulna R.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 -1.000000 0.000000 + anchor -0.006727 -0.026083 -0.013007 + } + device [ + RotationalMotor { + name "joint_radius_r" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_radius_r_sensor" + } + ] + endPoint Solid { + translation -0.006727 -0.026083 -0.013007 + children [ + DEF Radius_R_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Radius R.obj" + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0.000000 -1.000000 0.000000 + anchor -0.008797 -0.013610 -0.235841 + } + device [ + RotationalMotor { + name "joint_hand_r" + maxVelocity 628.318530718 + minPosition -3.141592741 + maxPosition 3.141592741 + maxTorque 10000000000.0 + } + PositionSensor { + name "joint_hand_r_sensor" + } + ] + endPoint Solid { + translation -0.008797 -0.013610 -0.235841 + children [ + DEF Hand_R_visual CadShape { + url "../sources/gait2392_simbody/meshes/obj/Hand R.obj" + } + ] + name "hand_r" + boundingObject DEF Hand_R_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Hand R.001.obj" + } + physics Physics { + density -1 + mass 0.457500 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 8.920000e-04 5.470000e-04 1.340000e-03 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "radius_r" + boundingObject DEF Radius_R_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Radius R.001.obj" + } + physics Physics { + density -1 + mass 0.607500 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 2.962000e-03 6.180000e-04 3.213000e-03 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "ulna_r" + boundingObject DEF Ulna_R_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Ulna R.001.obj" + } + physics Physics { + density -1 + mass 0.607500 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 2.962000e-03 6.180000e-04 3.213000e-03 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "humerus_r" + boundingObject DEF Humerus_R_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Humerus R.001.obj" + } + physics Physics { + density -1 + mass 2.032500 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 1.194600e-02 4.121000e-03 1.340900e-02 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name "torso" + boundingObject DEF Torso_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Torso.001.obj" + } + physics Physics { + density -1 + mass 26.826600 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 1.474500e+00 7.555000e-01 1.431400e+00 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } + } + ] + name IS name + boundingObject DEF Pelvis_001 Mesh { + url "../sources/gait2392_simbody/meshes/obj/Pelvis.001.obj" + } + physics Physics { + density -1 + mass 11.777000 + centerOfMass [ 0.000000 0.000000 0.000000 ] + inertiaMatrix [ + 1.028000e-01 8.710000e-02 5.790000e-02 + 0.000000e+00 0.000000e+00 0.000000e+00 + ] + } + } +} diff --git a/tests/expected/KukaLbrIiwa14R820.proto b/tests/expected/KukaLbrIiwa14R820.proto index 6a1f4b4..54634fe 100644 --- a/tests/expected/KukaLbrIiwa14R820.proto +++ b/tests/expected/KukaLbrIiwa14R820.proto @@ -1,4 +1,4 @@ -#VRML_SIM R2023a utf8 +#VRML_SIM R2023b utf8 # license: Apache License 2.0 # license url: http://www.apache.org/licenses/LICENSE-2.0 # This is a proto file for Webots for the KukaLbrIiwa14R820 diff --git a/tests/expected/MotomanSia20d.proto b/tests/expected/MotomanSia20d.proto index 086ba7b..b73c15a 100644 --- a/tests/expected/MotomanSia20d.proto +++ b/tests/expected/MotomanSia20d.proto @@ -1,4 +1,4 @@ -#VRML_SIM R2023a utf8 +#VRML_SIM R2023b utf8 # license: Apache License 2.0 # license url: http://www.apache.org/licenses/LICENSE-2.0 # This is a proto file for Webots for the MotomanSia20d @@ -60,7 +60,7 @@ PROTO MotomanSia20d [ translation 0.000000 0.000000 0.410000 rotation 0.000000 0.000000 1.000000 0.100000 children [ - Transform { + Pose { rotation -0.000000 0.000000 1.000000 3.141600 children [ Shape { @@ -95,7 +95,7 @@ PROTO MotomanSia20d [ endPoint Solid { rotation 0.000000 -1.000000 0.000000 0.100000 children [ - Transform { + Pose { rotation -1.000000 -0.000007 -0.000000 1.571593 children [ Shape { @@ -152,7 +152,7 @@ PROTO MotomanSia20d [ ] endPoint Solid { children [ - Transform { + Pose { rotation 1.000000 0.000050 0.000043 4.711593 children [ Shape { @@ -207,7 +207,7 @@ PROTO MotomanSia20d [ ] endPoint Solid { children [ - Transform { + Pose { rotation -1.000000 0.000000 0.000000 1.570000 children [ Shape { @@ -267,7 +267,7 @@ PROTO MotomanSia20d [ } ] name "link_b" - boundingObject Transform { + boundingObject Pose { rotation -1.000000 0.000000 0.000000 1.570000 children [ USE MOTOMAN_AXIS_B @@ -286,7 +286,7 @@ PROTO MotomanSia20d [ } ] name "link_u" - boundingObject Transform { + boundingObject Pose { rotation 1.000000 0.000050 0.000043 4.711593 children [ USE MOTOMAN_AXIS_U @@ -305,7 +305,7 @@ PROTO MotomanSia20d [ } ] name "link_l" - boundingObject Transform { + boundingObject Pose { rotation -1.000000 -0.000007 -0.000000 1.571593 children [ USE MOTOMAN_AXIS_L @@ -317,7 +317,7 @@ PROTO MotomanSia20d [ } ] name "link_s" - boundingObject Transform { + boundingObject Pose { rotation -0.000000 0.000000 1.000000 3.141600 children [ USE MOTOMAN_AXIS_S diff --git a/tests/expected/RobotWithDummyLink.proto b/tests/expected/RobotWithDummyLink.proto index 60104ba..38b149b 100644 --- a/tests/expected/RobotWithDummyLink.proto +++ b/tests/expected/RobotWithDummyLink.proto @@ -1,4 +1,4 @@ -#VRML_SIM R2023a utf8 +#VRML_SIM R2023b utf8 # license: Apache License 2.0 # license url: http://www.apache.org/licenses/LICENSE-2.0 # This is a proto file for Webots for the RobotWithDummyLink diff --git a/tests/expected/RobotWithLocalMeshes.proto b/tests/expected/RobotWithLocalMeshes.proto index fb294c7..5224eb5 100644 --- a/tests/expected/RobotWithLocalMeshes.proto +++ b/tests/expected/RobotWithLocalMeshes.proto @@ -1,4 +1,4 @@ -#VRML_SIM R2023a utf8 +#VRML_SIM R2023b utf8 # license: Apache License 2.0 # license url: http://www.apache.org/licenses/LICENSE-2.0 # This is a proto file for Webots for the RobotWithLocalMeshes diff --git a/tests/sources/gait2392_simbody/urdf/human.urdf b/tests/sources/gait2392_simbody/urdf/human.urdf index 5514659..27441ad 100644 --- a/tests/sources/gait2392_simbody/urdf/human.urdf +++ b/tests/sources/gait2392_simbody/urdf/human.urdf @@ -39,7 +39,7 @@ - + diff --git a/tests/test_export.py b/tests/test_export.py index 39be23b..c6e9f61 100644 --- a/tests/test_export.py +++ b/tests/test_export.py @@ -30,6 +30,12 @@ 'expected': [os.path.join(expectedDirectory, 'Human.proto')], 'arguments': '' }, + { + 'input': humanFilePath, + 'output': os.path.join(resultDirectory, 'HumanR2023a.proto'), + 'expected': [os.path.join(expectedDirectory, 'HumanR2023a.proto')], + 'arguments': '--target=R2023a' + }, { 'input': humanFilePath, 'output': os.path.join(resultDirectory, 'HumanR2022a.proto'), diff --git a/urdf2webots/importer.py b/urdf2webots/importer.py index dc1c132..5c18631 100755 --- a/urdf2webots/importer.py +++ b/urdf2webots/importer.py @@ -60,7 +60,7 @@ def mkdirSafe(directory): def convertUrdfFile(input=None, output=None, robotName=None, normal=False, boxCollision=False, toolSlot=None, initTranslation='0 0 0', initRotation='0 0 1 0', - initPos=None, linkToDef=False, jointToDef=False, relativePathPrefix=None, targetVersion='R2023a'): + initPos=None, linkToDef=False, jointToDef=False, relativePathPrefix=None, targetVersion='R2023b'): """Convert a URDF file into a Webots PROTO file or Robot node string.""" urdfContent = None if not input: @@ -95,7 +95,7 @@ def convertUrdfFile(input=None, output=None, robotName=None, normal=False, boxCo def convertUrdfContent(input, output=None, robotName=None, normal=False, boxCollision=False, toolSlot=None, initTranslation='0 0 0', initRotation='0 0 1 0', - initPos=None, linkToDef=False, jointToDef=False, relativePathPrefix=None, targetVersion='R2023a'): + initPos=None, linkToDef=False, jointToDef=False, relativePathPrefix=None, targetVersion='R2023b'): """ Convert a URDF content string into a Webots PROTO file or Robot node string. The current working directory will be used for relative paths in your URDF file. @@ -325,8 +325,8 @@ def convertUrdfContent(input, output=None, robotName=None, normal=False, boxColl parser.add_argument('--relative-path-prefix', dest='relativePathPrefix', default=None, help='If set and --input not specified, relative paths in your URDF file will be treated relatively ' 'to it rather than relatively to the current directory from which the script is called.') - parser.add_argument('--target', dest='targetVersion', default='R2023a', - choices=['R2023a', 'R2022b', 'R2022a', 'R2021b', 'R2021a', 'R2020b', 'R2020a'], + parser.add_argument('--target', dest='targetVersion', default='R2023b', + choices=['R2023b', 'R2023a', 'R2022b', 'R2022a', 'R2021b', 'R2021a', 'R2020b', 'R2020a'], help='Sets the Webots version the PROTO will target (will adapt which nodes will be used).') args = parser.parse_args() diff --git a/urdf2webots/parserURDF.py b/urdf2webots/parserURDF.py index e23b00f..9e4e144 100644 --- a/urdf2webots/parserURDF.py +++ b/urdf2webots/parserURDF.py @@ -17,7 +17,7 @@ # to pass from external robotName = '' -targetVersion = 'R2023a' +targetVersion = 'R2023b' class Inertia(): @@ -674,6 +674,10 @@ def getCollision(link, node, path, outputDirectory): collision.scale[0] = float(meshScale[0]) collision.scale[1] = float(meshScale[1]) collision.scale[2] = float(meshScale[2]) + if (targetVersion >= 'R2023b' and collision.scale[0] != 1.0 and collision.scale[1] != 1.0 + and collision.scale[2] != 1.0): + print('\033[1;33mWarning: BoundingObjects (collisions tags) cannot be scaled in version R2023b!' + ' Please create a separate model.\033[0m') if collision.scale[0] * collision.scale[1] * collision.scale[2] < 0.0: if extension in ['.dae', '.obj', '.stl']: collision.geometry.mesh.ccw = False diff --git a/urdf2webots/writeRobot.py b/urdf2webots/writeRobot.py index 001e931..bfea2ee 100644 --- a/urdf2webots/writeRobot.py +++ b/urdf2webots/writeRobot.py @@ -13,7 +13,7 @@ linkToDef = False jointToDef = False indexSolid = 0 -targetVersion = 'R2023a' +targetVersion = 'R2023b' class RGB(): @@ -145,7 +145,10 @@ def URDFLink(robotFile, link, level, parentList, haveChild = True robotFile.write((level + 1) * indent + 'children [\n') if hasattr(sensor, 'isImager') and sensor.isImager: - robotFile.write((level + 2) * indent + 'Transform {\n') + if (targetVersion >= 'R2023b'): + robotFile.write((level + 2) * indent + 'Pose {\n') + else: + robotFile.write((level + 2) * indent + 'Transform {\n') robotFile.write((level + 3) * indent + 'translation 0 0 0\n') robotFile.write((level + 3) * indent + 'rotation 0.577350 -0.577350 0.577350 2.094395\n') robotFile.write((level + 3) * indent + 'children [\n') @@ -261,8 +264,11 @@ def URDFBoundingObject(robotFile, link, level, boxCollision): for boundingObject in link.collision: initialIndent = boundingLevel * indent if hasGroup else '' if not boxCollision and (boundingObject.position != [0.0, 0.0, 0.0] or boundingObject.rotation[3] != 0.0 - or boundingObject.scale != [1.0, 1.0, 1.0]): - robotFile.write(initialIndent + 'Transform {\n') + or (targetVersion < 'R2023b' and boundingObject.scale != [1.0, 1.0, 1.0])): + if (targetVersion >= 'R2023b'): + robotFile.write(initialIndent + 'Pose {\n') + else: + robotFile.write(initialIndent + 'Transform {\n') if boundingObject.position != [0.0, 0.0, 0.0]: robotFile.write((boundingLevel + 1) * indent + 'translation %lf %lf %lf\n' % (boundingObject.position[0], boundingObject.position[1], @@ -272,7 +278,7 @@ def URDFBoundingObject(robotFile, link, level, boxCollision): boundingObject.rotation[1], boundingObject.rotation[2], boundingObject.rotation[3])) - if boundingObject.scale != [1.0, 1.0, 1.0]: + if boundingObject.scale != [1.0, 1.0, 1.0] and targetVersion < 'R2023b': robotFile.write((boundingLevel + 1) * indent + 'scale %lf %lf %lf\n' % (boundingObject.scale[0], boundingObject.scale[1], boundingObject.scale[2])) @@ -450,7 +456,10 @@ def URDFShape(robotFile, link, level, normal=False): for visualNode in link.visual: if visualNode.position != [0.0, 0.0, 0.0] or visualNode.rotation[3] != 0.0 or visualNode.scale != [1.0, 1.0, 1.0]: - robotFile.write(shapeLevel * indent + 'Transform {\n') + if (visualNode.scale != [1.0, 1.0, 1.0]): + robotFile.write(shapeLevel * indent + 'Transform {\n') + else: + robotFile.write(shapeLevel * indent + 'Pose {\n') if visualNode.position != [0.0, 0.0, 0.0]: robotFile.write((shapeLevel + 1) * indent + 'translation %lf %lf %lf\n' % (visualNode.position[0], visualNode.position[1],