正确实现带有向量输出端口的LeafSystem的困惑
Confusion About Implementing LeafSystem With Vector Output Port Correctly
我是一名自学 Drake 的学生,特别是 pydrake 与 Russ Tedrake 博士的优秀欠驱动机器人课程。我正在尝试编写一个组合的能量整形和 lqr 控制器来保持 cartpole 系统平衡直立。我的图表基于 Underactuated Robotics [http://underactuated.mit.edu/acrobot.html] 第 3 章中的 cartpole 示例,以及第 2 章中的 SwingUpAndBalanceController:[http:/ /underactuated.mit.edu/pend.html].
我发现由于使用 cart_pole.sdf model
,我必须创建一个抽象输入端口,以便从 cart_pole.get_output_port(0)
接收 FramePoseVector
。从那里我知道我必须创建 BasicVector 类型的控制信号输出以在馈入 cartpole 的驱动端口之前馈入饱和块。
我现在遇到的问题是,我不知道如何在DeclareVectorOutputPort
的回调函数中获取系统的当前状态数据。我假设我会在回调函数 OutputControlSignal
中使用 LeafContext
参数,获取 BasicVector
连续状态向量。然而,这个结果向量 x_bar
总是 NaN
。出于绝望(并测试以确保我的程序的其余部分正常工作),我将 x_bar
设置为控制器的初始化 cart_pole_context
并发现模拟运行时控制信号为 0.0(如预期)。我还可以将 output
设置为 100,然后 cartpole 模拟就会飞向无穷无尽的 space(正如预期的那样)。
TL;DR:在使用 DeclareVectorOutputPort
扩展 LeafSystem
的自定义控制器中获取连续状态向量的正确方法是什么?
感谢您的帮助!真的很感激:)我一直在自学所以有点辛苦哈哈。
# Combined Energy Shaping (SwingUp) and LQR (Balance) Controller
# with a simple state machine
class SwingUpAndBalanceController(LeafSystem):
def __init__(self, cart_pole, cart_pole_context, input_i, ouput_i, Q, R, x_star):
LeafSystem.__init__(self)
self.DeclareAbstractInputPort("state_input", AbstractValue.Make(FramePoseVector()))
self.DeclareVectorOutputPort("control_signal", BasicVector(1),
self.OutputControlSignal)
(self.K, self.S) = BalancingLQRCtrlr(cart_pole, cart_pole_context,
input_i, ouput_i, Q, R, x_star).get_LQR_matrices()
(self.A, self.B, self.C, self.D) = BalancingLQRCtrlr(cart_pole, cart_pole_context,
input_i, ouput_i,
Q, R, x_star).get_lin_matrices()
self.energy_shaping = EnergyShapingCtrlr(cart_pole, x_star)
self.energy_shaping_context = self.energy_shaping.CreateDefaultContext()
self.cart_pole_context = cart_pole_context
def OutputControlSignal(self, context, output):
#xbar = copy(self.cart_pole_context.get_continuous_state_vector())
xbar = copy(context.get_continuous_state_vector())
xbar_ = np.array([xbar[0], xbar[1], xbar[2], xbar[3]])
xbar_[1] = wrap_to(xbar_[1], 0, 2.0*np.pi) - np.pi
# If x'Sx <= 2, then use LQR ctrlr. Cost-to-go J_star = x^T * S * x
threshold = np.array([2.0])
if (xbar_.dot(self.S.dot(xbar_)) < 2.0):
#output[:] = -self.K.dot(xbar_) # u = -Kx
output.set_value(-self.K.dot(xbar_))
else:
self.energy_shaping.get_input_port(0).FixValue(self.energy_shaping_context,
self.cart_pole_context.get_continuous_state_vector())
output_val = self.energy_shaping.get_output_port(0).Eval(self.energy_shaping_context)
output.set_value(output_val)
print(output)
以下两点可能会有所帮助:
- 如果您想从
MultibodyPlant
获取车杆的状态,您 可能 想要连接到 continuous_state
输出端口,它给你一个法向量而不是抽象类型 FramePoseVector
。在这种情况下,您对 get_input_port().Eval(context)
的调用应该可以正常工作。
- 如果你做真的想读
FramePoseVector
,那么你必须稍微不同地评估输入端口。您可以找到 here. 的示例
我是一名自学 Drake 的学生,特别是 pydrake 与 Russ Tedrake 博士的优秀欠驱动机器人课程。我正在尝试编写一个组合的能量整形和 lqr 控制器来保持 cartpole 系统平衡直立。我的图表基于 Underactuated Robotics [http://underactuated.mit.edu/acrobot.html] 第 3 章中的 cartpole 示例,以及第 2 章中的 SwingUpAndBalanceController:[http:/ /underactuated.mit.edu/pend.html].
我发现由于使用 cart_pole.sdf model
,我必须创建一个抽象输入端口,以便从 cart_pole.get_output_port(0)
接收 FramePoseVector
。从那里我知道我必须创建 BasicVector 类型的控制信号输出以在馈入 cartpole 的驱动端口之前馈入饱和块。
我现在遇到的问题是,我不知道如何在DeclareVectorOutputPort
的回调函数中获取系统的当前状态数据。我假设我会在回调函数 OutputControlSignal
中使用 LeafContext
参数,获取 BasicVector
连续状态向量。然而,这个结果向量 x_bar
总是 NaN
。出于绝望(并测试以确保我的程序的其余部分正常工作),我将 x_bar
设置为控制器的初始化 cart_pole_context
并发现模拟运行时控制信号为 0.0(如预期)。我还可以将 output
设置为 100,然后 cartpole 模拟就会飞向无穷无尽的 space(正如预期的那样)。
TL;DR:在使用 DeclareVectorOutputPort
扩展 LeafSystem
的自定义控制器中获取连续状态向量的正确方法是什么?
感谢您的帮助!真的很感激:)我一直在自学所以有点辛苦哈哈。
# Combined Energy Shaping (SwingUp) and LQR (Balance) Controller
# with a simple state machine
class SwingUpAndBalanceController(LeafSystem):
def __init__(self, cart_pole, cart_pole_context, input_i, ouput_i, Q, R, x_star):
LeafSystem.__init__(self)
self.DeclareAbstractInputPort("state_input", AbstractValue.Make(FramePoseVector()))
self.DeclareVectorOutputPort("control_signal", BasicVector(1),
self.OutputControlSignal)
(self.K, self.S) = BalancingLQRCtrlr(cart_pole, cart_pole_context,
input_i, ouput_i, Q, R, x_star).get_LQR_matrices()
(self.A, self.B, self.C, self.D) = BalancingLQRCtrlr(cart_pole, cart_pole_context,
input_i, ouput_i,
Q, R, x_star).get_lin_matrices()
self.energy_shaping = EnergyShapingCtrlr(cart_pole, x_star)
self.energy_shaping_context = self.energy_shaping.CreateDefaultContext()
self.cart_pole_context = cart_pole_context
def OutputControlSignal(self, context, output):
#xbar = copy(self.cart_pole_context.get_continuous_state_vector())
xbar = copy(context.get_continuous_state_vector())
xbar_ = np.array([xbar[0], xbar[1], xbar[2], xbar[3]])
xbar_[1] = wrap_to(xbar_[1], 0, 2.0*np.pi) - np.pi
# If x'Sx <= 2, then use LQR ctrlr. Cost-to-go J_star = x^T * S * x
threshold = np.array([2.0])
if (xbar_.dot(self.S.dot(xbar_)) < 2.0):
#output[:] = -self.K.dot(xbar_) # u = -Kx
output.set_value(-self.K.dot(xbar_))
else:
self.energy_shaping.get_input_port(0).FixValue(self.energy_shaping_context,
self.cart_pole_context.get_continuous_state_vector())
output_val = self.energy_shaping.get_output_port(0).Eval(self.energy_shaping_context)
output.set_value(output_val)
print(output)
以下两点可能会有所帮助:
- 如果您想从
MultibodyPlant
获取车杆的状态,您 可能 想要连接到continuous_state
输出端口,它给你一个法向量而不是抽象类型FramePoseVector
。在这种情况下,您对get_input_port().Eval(context)
的调用应该可以正常工作。 - 如果你做真的想读
FramePoseVector
,那么你必须稍微不同地评估输入端口。您可以找到 here. 的示例