Enforce fixed constraints recursively when setting pose on freegroups

[azeey]

Suggestion for #632

Testing the transform tool on three models attached with a detachable joint shows the problem. Without this change, we observe the previous rotation behavior.

detachable_joint_three_boxes.sdf

<?xml version="1.0"?>
<sdf version="1.11">
  <world name="default">
  <light type="directional" name="sun">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>1 1 1 1</diffuse>
      <specular>0.5 0.5 0.5 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>

    <model name="ground_plane">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
            </plane>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
        </visual>
      </link>
    </model>
    <model name="M1">
      <pose>0 2 3.0 0 0.0 0.0</pose>
      <link name="body">
        <inertial auto="true"/>
        <collision name="box_collision">
          <geometry>
            <box>
              <size>1 1 0.5</size>
            </box>
          </geometry>
        </collision>

        <visual name="box_visual">
          <geometry>
            <box>
              <size>1 1 0.5</size>
            </box>
          </geometry>
          <material>
            <ambient>1 0 0 1</ambient>
            <diffuse>1 0 0 1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
        </visual>
      </link>
      <plugin filename="gz-sim-detachable-joint-system"
              name="gz::sim::systems::DetachableJoint">
       <parent_link>body</parent_link>
       <child_model>M2</child_model>
       <child_link>body2</child_link>
      </plugin>
    </model>

    <model name="M2">
      <pose>3.3 2 3.0 0 0.0 0</pose>
      <link name="body2">
        <inertial auto="true"/>
        <collision name="box_collision">
          <geometry>
            <box>
              <size>1 1 0.5</size>
            </box>
          </geometry>
        </collision>

        <visual name="box_visual">
          <geometry>
            <box>
              <size>1 1 0.5</size>
            </box>
          </geometry>
          <material>
            <ambient>0 1 0 1</ambient>
            <diffuse>0 1 0 1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
        </visual>
      </link>
      <plugin filename="gz-sim-detachable-joint-system"
              name="gz::sim::systems::DetachableJoint">
       <parent_link>body2</parent_link>
       <child_model>M3</child_model>
       <child_link>body3</child_link>
      </plugin>
    </model>
    <model name="M3">
      <pose>8.3 2 3.0 0 0.0 0</pose>
      <link name="body3">
        <inertial auto="true"/>
        <collision name="box_collision">
          <geometry>
            <box>
              <size>1 1 0.5</size>
            </box>
          </geometry>
        </collision>

        <visual name="box_visual">
          <geometry>
            <box>
              <size>1 1 0.5</size>
            </box>
          </geometry>
          <material>
            <ambient>0 1 0 1</ambient>
            <diffuse>0 1 0 1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
        </visual>
      </link>
    </model>
  </world>
</sdf>

[iche033]

thanks! I can reproduce the issue and confirm this fixes it for me.