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) = ManipulatorDynamics(self.tree, q, v) # Desired pendulum parameters. l = 2. b = .1 # Control gains for stabilizing the second joint. kp = 1 kd = .1 # Cancel double pend dynamics and inject single pend dynamics. torque[:] = Cv - tauG + \ M.dot([-self.g/l*sin(q[0]) - b*v[0], -kp*q[1]+kd*v[1] ])
def DoCalcVectorOutput( self, context, controller_input, # state of base + pendulum, pendulum torque controller_state, # unused input (static controller) controller_output # force on base + torque on pendulum ): # unpack state q = controller_input[:2] # base position + pendulum angle q_dot = controller_input[2:4] # time derivative of q # extract manipulator equations: M*a + Cv = tauG + B*u + tauExt # (for this system B is the identity and the external forces tauExt are zero # hence, for simplicity, we will just drop them from the code) M, Cv, tauG, B, tauExt = ManipulatorDynamics(self.vibrating_pendulum, q, q_dot) Minv = np.linalg.inv(M) tau = tauG - Cv # desired acceleration of the base # note that this depends on time t = context.get_time() a_base = - omega**2 * h * np.sin(omega * t) # cancel out the dynamics of the pendulum # and enforce harmonic motion to the base # (to fully explain these lines we would need a small math derivation, # since this is not the goal of the exercise we skip it, # if you want, you can try your own, it is not hard) torque = controller_input[-1] force = - tau[0] # cancel gravity, centrifugal, and Coriolis force += - (tau[1] + torque) * Minv[0,1] / Minv[0,0] # cancel pendulum effects on the base force += a_base / Minv[0,0] # enforce desired acceleration # control signal controller_output[:] = [force, torque]
from pydrake.all import (FloatingBaseType, RigidBodyTree) from underactuated import (FindResource, ManipulatorDynamics) tree = RigidBodyTree(FindResource("double_pendulum/double_pendulum.urdf"), FloatingBaseType.kFixed) q = (1., 1.) v = (0.1, 0.1) (M, Cv, tauG, B) = ManipulatorDynamics(tree, q, v) print("M = " + str(M)) print("Cv = " + str(Cv)) print("tauG = " + str(tauG)) print("B = " + str(B))
from pydrake.all import MultibodyPlant, Parser, UniformGravityFieldElement from underactuated import FindResource, ManipulatorDynamics plant = MultibodyPlant() parser = Parser(plant) parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf")) plant.AddForceElement(UniformGravityFieldElement()) plant.Finalize() q = [0.1, 0.1] v = [1, 1] (M, Cv, tauG, B, tauExt) = ManipulatorDynamics(plant, q, v) print("M = \n" + str(M)) print("Cv = " + str(Cv)) print("tau_G = " + str(tauG)) print("B = " + str(B)) print("tau_ext = " + str(tauExt)) # TODO(russt): add symbolic version pending resolution of drake #11240