search

RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.25k stars 1.25k forks source link

[urdf parsing] Multi-body plant with planar joint, state axis rotated #20095

Closed FranekStark closed 1 year ago

FranekStark commented 1 year ago

What happened?

I am using drake to simulate a planar floating system. The system is connected to the world via the planar_joint:

  <joint name="joint_world_base" type="planar">
    <axis xyz="0 0 1"/>
    <child link="base_link"/> 
    <parent link="world"/>
  </joint>

The system has one additional joint, hence the dimension of the state vector is 8. The method plant.GetStateNames() outputs: 

['SYSTEM_joint_world_base_x', 'SYSTEM_joint_world_base_y', 'SYSTEM_joint_world_base_qz', 
 SYSTEM_joint_base_reaction_wheel_q', 'SYSTEM_joint_world_base_vx', 'SYSTEM_joint_world_base_vy', 'SYSTEM_joint_world_base_wz', 'SYSTEM_joint_base_reaction_wheel_w']

I now set the initial state:

diagram.GetMutableSubsystemState(plant, context).get_mutable_continuous_state().SetFromVector([-2.,0,0,0,0,0,0,0])

Which in my opinion reflects x=-2., y=0. And systems orientation and velocities (also of the additional joint) equal 0. When I now look into the meshcat visualization it looks the following: image For me it looks like the x=-2 became y = 2. However, when I get the current state via plant.get_state_output_port() it says [-2.,0,0,0,0,0,0,0]. Nevertheless, any forces calculated by a controller and applied to the system are applied in the "rotated" version (also visualized in Meshcat), hence the system is not stabilizable. I can only solve this problem If I multiply the state with the following matrix before giving it to the controller (which then reflects the state as it is also displayed in meshcat):

        [0, 1, 0, 0, 0, 0, 0, 0],
        [-1, 0, 0, 0, 0, 0, 0, 0],
        [0, 0, 1, 0, 0, 0, 0, 0],
        [0, 0,  0, 1, 0, 0, 0],
        [0, 0, 0, 0, 0, 1, 0, 0],
        [0, 0, 0, 0, -1, 0, 0, 0],
        [0, 0, 0, 0, 0, 0, 1, 0],
        [0, 0, 0, 0, 0, 0, 0, 1]

Version

20230713164657 b35fa8277c449aee51024adc5103c2bbaf330f08

What operating system are you using?

Ubuntu 22.04

What installation option are you using?

apt install drake

Relevant log output

No response

RussTedrake commented 1 year ago

Are you able to share the entire urdf with us? I suspect there is another transformation upstream from the planar joint?

FranekStark commented 1 year ago

Sure, this is the whole urdf:

<?xml version="1.0"?>
<robot name="SYSTEM">
   <material name="RED">
    <color rgba="1.0 0 0 1.0"/>
  </material>

   <material name="GREEN">
    <color rgba="0.0 1.0 0 1.0"/>
  </material>
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="1.0" radius="0.35"/>
      </geometry>
    </visual>
    <inertial>
        <mass value="198.8"/>
        <inertia ixx="24.80417083" ixy="0.0" ixz="0.0" iyy="24.80417083" iyz="0.0" izz="12.176"/>
    </inertial>
  </link>

  <link name="reaction_wheel">
     <visual>
      <geometry>
        <cylinder length="0.05" radius="0.1480"/>
      </geometry>
    </visual>
     <inertial>
        <mass value="4.01"/>
        <inertia ixx="0.02438579" ixy="0.0" ixz="0.0" iyy="0.02438579" iyz="0.0" izz="0.047"/>
    </inertial>
  </link>

  <link name="base_marker_x">
     <visual>
     <material name="RED" />
     <origin xyz="0.2 0 0.5" rpy="0 0 0" />
     <geometry>
        <box size="0.4 0.05 0.05"/>
     </geometry>
    </visual>
  </link>

  <link name="base_marker_y">
     <visual>
     <material name="GREEN" />
     <origin xyz="0 0.2 0.5" rpy="0 0 0" />
     <geometry>
        <box size="0.05 0.4 0.05"/>
     </geometry>
    </visual>
  </link>

   <joint name="joint_base_marker_x" type="fixed">
    <origin xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="base_marker_x"/>
  </joint>

     <joint name="joint_base_marker_y" type="fixed">
    <origin xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="base_marker_y"/>
  </joint>

  <link name="reaction_wheel_point">
     <visual>
     <material name="RED" />
      <geometry>
        <cylinder length="0.1" radius="0.01"/>
      </geometry>
    </visual>
  </link>

  <joint name="joint_reaction_wheel_point" type="fixed">
    <origin xyz="0 0.05 0.05"/>
    <parent link="reaction_wheel"/>
    <child link="reaction_wheel_point"/>
  </joint>

  <link name="f0">
    <visual>
    <origin xyz="0.05 0 0" rpy="0 0 0" />
    <material name="RED"/>
    <geometry>
        <box size="0.1 0.05 0.05"/>
    </geometry>
    </visual>
  </link>

  <link name="f1">
    <visual>
    <origin xyz="0.05 0 0" rpy="0 0 0" />
    <material name="GREEN"/>
    <geometry>
        <box size="0.1 0.05 0.05"/>
    </geometry>
    </visual>
  </link>

  <link name="f2">
    <visual>
    <origin xyz="0.05 0 0" rpy="0 0 0" />
    <material name="RED"/>
    <geometry>
        <box size="0.1 0.05 0.05"/>
    </geometry>
    </visual>
  </link>

  <link name="f3">
    <visual>
    <origin xyz="0.05 0 0" rpy="0 0 0" />
    <material name="GREEN"/>
    <geometry>
        <box size="0.1 0.05 0.05"/>
    </geometry>
    </visual>
  </link>

  <link name="f4">
    <visual>
    <origin xyz="0.05 0 0" rpy="0 0 0" />
    <material name="RED"/>
    <geometry>
        <box size="0.1 0.05 0.05"/>
    </geometry>
    </visual>
  </link>

  <link name="f5">
    <visual>
    <origin xyz="0.05 0 0" rpy="0 0 0" />
    <material name="GREEN"/>
    <geometry>
        <box size="0.1 0.05 0.05"/>
    </geometry>
    </visual>
  </link>

  <link name="f6">
    <visual>
    <origin xyz="0.05 0 0" rpy="0 0 0" />
    <material name="RED"/>
    <geometry>
        <box size="0.1 0.05 0.05"/>
    </geometry>
    </visual>
  </link>

  <link name="f7">
    <visual>
    <origin xyz="0.05 0 0" rpy="0 0 0" />
    <material name="GREEN"/>
    <geometry>
        <box size="0.1 0.05 0.05"/>
    </geometry>
    </visual>
  </link>

  <joint name="joint_base_reaction_wheel" type="continuous">
    <origin xyz="0 0 0.6"/>
    <axis xyz="0 0 1"/>
    <parent link="base_link"/>
    <child link="reaction_wheel"/>
    <limit velocity="25.0" effort="1.5"/>
  </joint>

  <transmission name="transmission_reaction_wheel">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_base_reaction_wheel">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="actuator_base_reaction_wheel">
      <mechanicalReduction>1.0</mechanicalReduction>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>

  <joint name="joint_world_base" type="planar">
    <axis xyz="0 0 1"/>
    <child link="base_link"/> 
    <parent link="world"/>
  </joint>

  <joint name="joint_base_f0" type="fixed">
    <parent link="base_link"/>
    <child link="f0"/>
    <origin xyz="0.35 0 0" rpy="0 0 1.57079632679"/>
  </joint>

  <joint name="joint_base_f1" type="fixed">
    <parent link="base_link"/>
    <child link="f1"/>
    <origin xyz="0.35 0 0" rpy="0 0 -1.57079632679"/>
  </joint>

  <joint name="joint_base_f2" type="fixed">
    <parent link="base_link"/>
    <child link="f2"/>
    <origin xyz="0 0.35 0" rpy="0 0 3.14159"/>
  </joint>

  <joint name="joint_base_f3" type="fixed">
    <parent link="base_link"/>
    <child link="f3"/>
    <origin xyz="0 0.35 0" rpy="0 0 0"/>
  </joint>

  <joint name="joint_base_f4" type="fixed">
    <parent link="base_link"/>
    <child link="f4"/>
    <origin xyz="-0.35 0 0" rpy="0 0 -1.57079632679"/>
  </joint>

  <joint name="joint_base_f5" type="fixed">
    <parent link="base_link"/>
    <child link="f5"/>
    <origin xyz="-0.35 0 0" rpy="0 0 1.57079632679"/>
  </joint>

  <joint name="joint_base_f6" type="fixed">
    <parent link="base_link"/>
    <child link="f6"/>
    <origin xyz="0 -0.35 0" rpy="0 0 0"/>
  </joint>

  <joint name="joint_base_f7" type="fixed">
    <parent link="base_link"/>
    <child link="f7"/>
    <origin xyz="0 -0.35 0" rpy="0 0 3.14159"/>
  </joint>

</robot>
sherm1 commented 1 year ago

My first thought is that this is a parsing bug. The corresponding MultibodyPlant planar joint relates a frame on the parent to a frame on the child. The parser needs to take the urdf "normal" vector as the z axis and use it to produce the appropriate frames with x and y axes also. Apparently the frames are not oriented in the expected manner.

Assigning to @rpoyner-tri for disposition.

jwnimmer-tri commented 1 year ago

FYI https://drake.mit.edu/pydrake/pydrake.visualization.model_visualizer.html is an easy way to load a URDF and slide the joints around to see the kinematics. It can also do --visualize_frames to show each body frame.

RussTedrake commented 1 year ago

I agree that something looks wrong with the planar joint. This minimal URDF is enough to show it:

<?xml version="1.0"?>
<robot name="SYSTEM">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="1.0" radius="0.35"/>
      </geometry>
    </visual>
  </link>

  <joint name="joint_world_base" type="planar">
    <axis xyz="0 0 1"/>
    <child link="base_link"/> 
    <parent link="world"/>
  </joint>

</robot>

The joint appears to be rotated 90 degrees around z compared to what I would expect.

RussTedrake commented 1 year ago

I think the problem is here, because

from pydrake.all import RotationMatrix
print(RotationMatrix.MakeFromOneVector([0, 0, 1], 2))

displays

RotationMatrix([
  [0.0, 1.0, 0.0],
  [-1.0, -0.0, 0.0],
  [0.0, -0.0, 1.0],
])

where I think the parser was hoping for the identity matrix.

FranekStark commented 1 year ago

@RussTedrake thanks for investigating this. The rotation matrix clearly matches the one I use to re-transform the states.

RussTedrake commented 1 year ago

I think that the URDF spec is actually ambiguous about the orientation of the planar joint's x-y axes (one axis is insufficient to define the frame). But I agree that our documentation for PlanarJoint (and our coordinate names) clearly suggest that axis=[0, 0, 1] should give the first coordinate aligned with the x-axis.

My proposal for a better solution would be to use the rotation matrix which aligns that z-axis with axis but is the minimum rotation from the identity matrix. That seems canonical to me?

sherm1 commented 1 year ago

I'm not sure that's the right heuristic. Consider axis=[-1, 0, 0] where the plane normal (z axis) is in the -x direction. Not clear to me how to apply "closest to identity matrix" in that case. How about something like:

That would map the axis to meaning of q0 q1 q2 like this: z -> x y z, -z -> -x y -z, x -> y z x, -x -> -y z -x, etc. (we require a right-handed set) I'm not sure any user would have an obvious expectation for any cases other than the normal=z case. @FranekStark what would your expectations be if you had specified a different normal direction?

sherm1 commented 1 year ago

If we can find a nicer heuristic we could embed it in RotationMatrix::MakeFromOneVector() so the existing code wouldn't have to change. WDYT @mitiguy ?

xuchenhan-tri commented 1 year ago

The closest to identity frame seems intuitive to me. The rotation from identity to the desired basis with the prescribed z would just be the rotation with axis new-z cross old-z and with angle as small as possible

Consider axis=[-1, 0, 0] where the plane normal (z axis) is in the -x direction. Not clear to me how to apply "closest to identity matrix" in that case. How about something like:

In this case, the new x would be the old z, y stays the same and the new z is -old x as required.

xuchenhan-tri commented 1 year ago

Of course there is issue when axis = [0, 0, -1]. For that we have to choose some convention.

FranekStark commented 1 year ago

@sherm1 it is actually a good question what I did expect to happen. So my first guess was the following: The axis that is set in the planar joint is the one that is basically 'blocked'. Hence I was expecting that I get a 'normal' state vector that is just missing the z-component in my case. So if the specified axis is one of the R3 standard base vectors this is easy to implement. What now happens if this is not the case is actually a good question. Maybe put it like that: It is only allowed to specify the axis as [1,0,0], [0,1,0], [0,0,1]. Otherwise, the joint must be rotated, hence it is again clear where is which axis. Does this make sense?

sherm1 commented 1 year ago

@FranekStark there isn't actually any restriction on how the planar joint can be oriented. They aren't restricted to World as a parent and can be oriented arbitrarily as a connection between two links. What I'd like to see is a heuristic that delivers what you expect in the obvious case(s) and does something reasonable in other cases. The x-y directions are arbitrary and unspecified by urdf, so we can pick them however makes sense. Any directions for x-y are legal, but following the "principle of least astonishment" we ought to favor ones that a user would expect.

jwnimmer-tri commented 1 year ago

For the record, see also #19355 and #19314 for the backstory.