Odd Linearization of Pydrake Body with Bushing Constraint

Question

I'm currently simulating a four-bar linkage with the bottom link bolted to the ground, and I am using a bushing constraint. I need to use the Linearize function in order to linearize the dynamics in the form x' = Ax + Bu, around the upright square position. When I run the simulation, it stays upright for a decent amount of time, so I would expect Ax to be relatively close to 0 at the start. However, when I print out Ax, it's very large: [ 0. 0. 0. 0.558.35230052 -1263.24630058 98.52502957 -49055.95676526]

I was wondering if anyone has any idea why this is happening?

Below is the most barebones I could make the python file:

from time import sleep
import numpy as np
from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder,
                         Parser, Linearize, OutputPortSelection,
                         LinearBushingRollPitchYaw, StartMeshcat,
                         FrameIndex, LinearBushingRollPitchYaw,
                         MeshcatVisualizerCpp, ConstantVectorSource,
                         Simulator)

meshcat = StartMeshcat()

# unstable equilibrium point
x = [np.pi / 2, np.pi / 2, np.pi / 2, np.pi / 2, 0, 0, 0, 0]

# start construction site of our block diagram
builder = DiagramBuilder()

# instantiate the fourbar linkage and the scene graph
fourbar, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
urdf_path = 'fourbar_grounded.urdf'
object_instance = Parser(fourbar).AddModelFromFile(urdf_path)

frameA = fourbar.get_frame(FrameIndex(2))
frameC = fourbar.get_frame(FrameIndex(6))
torque_stiffness = np.array([1000.0, 1000.0, 1000.0])
torque_damping = np.array([100.0, 100.0, 100.0])
force_stiffness = np.array([4000.0, 4000.0, 4000.0])
force_damping = np.array([100.0, 100.0, 100.0])
lbh = LinearBushingRollPitchYaw(
    frameA, frameC, torque_stiffness, torque_damping, force_stiffness, force_damping)
bushing = fourbar.AddForceElement(lbh)

fourbar.Finalize()

# set the operating point (vertical unstable equilibrium)
context = fourbar.CreateDefaultContext()
context.get_mutable_continuous_state_vector().SetFromVector(x)

# fix the input port to zero and get its index for the lqr function
fourbar.get_actuation_input_port().FixValue(context, [0])
input_i = fourbar.get_actuation_input_port().get_index()
linear_system = Linearize(
    fourbar, context, input_i,
    OutputPortSelection.kNoOutput, equilibrium_check_tolerance=10)
A = linear_system.A()
print("Ax:", np.dot(A, x))

# set up no actuation
ci_system = builder.AddSystem(ConstantVectorSource([0]))
builder.Connect(ci_system.get_output_port(),
                fourbar.get_actuation_input_port())


# add a visualizer and wire it
visualizer = MeshcatVisualizerCpp.AddToBuilder(
    builder, scene_graph, meshcat)

diagram = builder.Build()
simulator = Simulator(diagram)


def simulate_and_animate(x0, sim_time=20):

    # start recording the video for the animation of the simulation
    visualizer.StartRecording()

    # reset initial time and state
    context = simulator.get_mutable_context()
    context.SetTime(0.)
    context.SetContinuousState(x0)

    # run sim
    simulator.Initialize()
    simulator.AdvanceTo(sim_time)

    # stop video
    visualizer.StopRecording()
    visualizer.PublishRecording()


simulate_and_animate(x)
sleep(100)

And this is the urdf file:

<?xml version="1.0"?>
<robot name="FourBar">

  <material name="green">
    <color rgba=".3 .6 .4 1"/>
  </material>
  <material name="red">
    <color rgba=".6 .4 0.3 1"/>
  </material>
  <link drake_ignore="true" name="world">
    <inertial>
      <!-- drc-viewer needs this to have inertia to parse properly. Remove it when that bug is fixed. -->
      <origin xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
  </link>

  <link name="base">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
  </link>
  <link name="base_link">
    <inertial>
      <mass value="1"/>
      <origin xyz="0 0 -0.125" rpy="0 0 0"/>
      <inertia ixx="1.25" ixy="0" ixz="0" iyy="1.25" iyz="0" izz=".125"/>
    </inertial>
    <visual>
      <origin xyz="0 0 -0.125" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.25" radius="0.0125"/>
      </geometry>
      <material name="green"/>
    </visual>
  </link>
  <link name="link_1">
    <inertial>
      <mass value="1"/>
      <inertia ixx="1.25" ixy="0" ixz="0" iyy="1.25" iyz="0" izz=".125"/>
    </inertial>
    <visual>
      <origin xyz="0 0 -0.125" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.25" radius="0.0125"/>
      </geometry>
      <material name="red"/>
    </visual>
  </link>
  <link name="link_2">
    <inertial>
      <mass value="1"/>
      <origin xyz="0 0 -0.125" rpy="0 0 0"/>
      <inertia ixx="1.25" ixy="0" ixz="0" iyy="1.25" iyz="0" izz=".125"/>
    </inertial>
    <visual>
      <origin xyz="0 0 -0.125" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.25" radius="0.0125"/>
      </geometry>
      <material name="green"/>
    </visual>
  </link>
  <link name="link_3">
    <inertial>
      <mass value="1"/>
      <origin xyz="0 0 -0.125" rpy="0 0 0"/>
      <inertia ixx="1.25" ixy="0" ixz="0" iyy="1.25" iyz="0" izz=".125"/>
    </inertial>
    <visual>
      <origin xyz="0 0 -0.125" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.25" radius="0.0125"/>
      </geometry>
      <material name="red"/>
    </visual>
  </link>
  <link name="link_4">
    <inertial>
      <mass value=".1"/>
      <origin xyz="0 0 -0.125" rpy="0 0 0"/>
      <inertia ixx=".125" ixy="0" ixz="0" iyy=".125" iyz="0" izz=".0125"/>
    </inertial>
    <!--
    <visual>
      <origin xyz="0 0 -0.125" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.25" radius="0.0125"/>
      </geometry>
      <material name="red"/>
    </visual>
    -->
  </link>
  <joint name="weld" type="fixed">
    <parent link="world"/>
    <child link="base"/>
    <origin xyz="0 0 0.0"/>
  </joint>
  <joint name="base_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <parent link="base"/>
    <child link="base_link"/>
    <origin xyz="0 0 0" rpy="0 1.57079632679 0"/>
  </joint>
  <joint name="joint_0" type="continuous">
    <axis xyz="0 1 0"/>
    <parent link="base_link"/>
    <child link="link_1"/>
    <limit effort="30" lower="0.4" upper="2" />
    <origin rpy="0 0 0" xyz="0 0 -0.25"/>
  </joint>
  <joint name="joint_1" type="continuous">
    <axis xyz="0 1 0"/>
    <parent link="link_1"/>
    <child link="link_2"/>
    <limit effort="30" lower="0.4" upper="2" />
    <origin rpy="0 0 0" xyz="0 0 -0.25"/>
  </joint>
  <joint name="joint_2" type="continuous">
    <axis xyz="0 1 0"/>
    <parent link="link_2"/>
    <child link="link_3"/>
    <origin rpy="0 0 0" xyz="0 0 -0.25"/>
    <limit effort="30" lower="0.4" upper="2"/>
  </joint>
  <joint name="joint_3" type="continuous">
    <axis xyz="0 1 0"/>
    <parent link="link_3"/>
    <child link="link_4"/>
    <origin rpy="0 0 0" xyz="0 0 -0.25"/>
  </joint>
  <transmission type="SimpleTransmission" name="joint_0_trans">
    <actuator name="control_torque0"/>
    <joint name="joint_0"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
</robot>

Answer 1

Currently you are multiplying it by a big penalty force which is linear in the bushing coordinates but nonlinear in the original coordinates. I would think that for your purposes, you would want to linearize the dynamics without the bushing element -- I verified that this gives the beautiful linearization that you would expect. And then you would want to project the linearization onto the manifold of your constraint (which here is simply: q0=q2, q1=q3, q0+q1=PI) to get the linearization in the one true degree of freedom.

Answer 2

I just wasn't thinking hard about this; you have to subtract x0 from the input to A, so I'm really getting what the linearization believes x' to be at 2x0.