Search code examples
drake

Robot unexpectedly forms closed loop

I am working on balancing a leg using LQR. After reading the answers on the following thread I've decided to go about the problem in the same way, i.e. constraining the foot to the ground via pin joint:

How to use an LQR controller with collision geometry

To do this, I import two urdf files, one for the robot and one for the ground which leads to the robot topology as shown in the picture below.

enter image description here

The error occurs when I try to connect foot to the ground. I try to do this with the following piece of code:

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
leg_instance = Parser(plant).AddModelFromFile("HoppingLeg/leg_v2/urdf/LEG_002_Foot_Hinge.urdf")
ground_instance = Parser(plant).AddModelFromFile("HoppingLeg/leg_v2/urdf/ground.urdf")

frame_foot = plant.GetFrameByName("Link_foot")
frame_ground = plant.GetFrameByName("ground")
joint_ground = RevoluteJoint(name="foot_ground",
                            frame_on_parent=frame_foot,
                            frame_on_child=frame_ground,
                            axis=np.array([1., 0., 0.]),
                            damping=0.0)

plant.AddJoint(joint_ground)
 
plant.Finalize()

After which drake tells me that I am forming a closed loop robot.

RuntimeError: This mobilizer is creating a closed loop since the outboard body already has an inboard mobilizer connected to it. If a physical loop is really needed, consider using a constraint instead.

I don't understand why adding this link leads to a closed loop robot as my base_link is not connected to the world or the ground. Additionally, I am not sure whether the direction of the arrows in the diagram generated by drake matter, but in the robot leg urdf, base_link is the parent link to which the other links are connected to. For example this is my join between base_link and Link_rail_y:

 <joint
    name="rail_y"
    type="prismatic">
    <origin
      xyz="0 0 0"
      rpy="0 0 0" />
    <parent
      link="base_link" />
    <child
      link="Link_rail_y" />
    <axis
      xyz="0 1 0" />
    <limit
      lower="-1.5"
      upper="1.5" />
  </joint>
Solution

  • The solution is to reverse the parent-child relationship so that the Link_Knee_pitch is a child of the Link_foot, etc. Right now, we only allow each body to have a single parent.

    We should do this for you automatically, but haven't supported it yet. See https://github.com/RobotLocomotion/drake/issues/17429