RussTedrake / underactuated

The course text for MIT 6.832 (and 6.832x on edX)
Other
701 stars 203 forks source link

Feedback cancellation doesn't work due to tauExt term (underactuated/intro.pynb) #531

Closed ikonomoug closed 9 months ago

ikonomoug commented 10 months ago
def DoCalcVectorOutput(self, context, double_pend_state, unused, torque):
        # Extract manipulator dynamics.
        q = double_pend_state[:2]
        v = double_pend_state[-2:]
        (M, Cv, tauG, B, tauExt) = ManipulatorDynamics(self.plant, q, v)

        # Desired pendulum parameters.
        length = 2.0
        b = 0.1

        # Control gains for stabilizing the second joint.
        kp = 1
        kd = 0.1

        # Cancel double pend dynamics and inject single pend dynamics.
        torque[:] = (
            Cv
            - tauG
            - tauExt
            + M.dot(
                [
                    self.g / length * np.sin(q[0]) - b * v[0],
                    -kp * q[1] - kd * v[1],
                ]
            )
        )

I'm not familiar with how drake works but tauExt (which does not appear in the text) makes the controller useless. Commenting it out fixes everything. Maybe I'm doing something wrong but this is the only way I could fix it.

RussTedrake commented 9 months ago

Thanks for catching that. This changed very recently. I've opened https://github.com/RobotLocomotion/drake/issues/20244 to point to the issue in the drake docs, and properly subtracted tau_g from tau_ext in this repo.