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