逆动力学控制器 Drake

问题描述

我目前在我的工厂中有一个对象的所需位置与时间的数组。我正在使用逆动力学控制器来将对象驱动到这个所需的位置，但我遇到了一些困难。这是我的做法：

1. 我创建了控制器系统

ID_cont = InverseDynamicsController(robot=controller_plant,kp=np.array([0.5]),ki=np.array([0.3]),kd=np.array([0.4]),has_reference_acceleration=False)
ID_controller = builder.AddSystem(ID_cont)

2. 我得到了控制器输入和输出端口

    control_estimated_state_input_port = ID_controller.get_input_port(0)
    control_desired_state_input_port = ID_controller.get_input_port(1)
    control_output_port = ID_controller.get_output_port(0)

3. 我添加了一个常量状态源（可能做错了）和一个状态插值器

    constant_state_source = ConstantVectorSource(np.array([0.0]))
    builder.AddSystem(constant_state_source)
    position_to_state = StateInterpolatorWithdiscreteDerivative(controller_plant.num_positions(),controller_plant.time_step())
    builder.AddSystem(position_to_state)

4. 我将控制器连接到工厂

    builder.Connect(constant_state_source.get_output_port(),position_to_state.get_input_port())
    builder.Connect(position_to_state.get_output_port(),control_desired_state_input_port)
    builder.Connect(plant.get_state_output_port(model_instance_1),control_estimated_state_input_port)
    builder.Connect(control_output_port,plant.get_actuation_input_port(model_instance_1))

5. 接下来，我正在尝试创建一个 while 循环来推进模拟并更改“恒定向量源”，以便我可以输入我的位置与时间值，但我不确定这是否不起作用是因为这是完全错误的方法，或者这是正确的方法，但我只是有一些错误

    diagram_context = diagram.CreateDefaultContext()
    sim_time_temp = diagram_context.get_time()
    time_step = 0.1

    while sim_time_temp < duration:
        ID_controller_context = diagram.GetMutableSubsystemContext(ID_controller,diagram_context)
        simulator.Advanceto(sim_time_temp)
        sim_time_temp = sim_time_temp + time_step

解决方法

3. 我添加了一个常量状态源（可能做错了）和一个状态插值器

正如您所怀疑的那样，如果您已经有了希望系统跟踪的位置和时间序列，这不是最好的方法。相反，您应该使用 TrajectorySource。由于您有一组位置样本 positions（num_times x num_positions 数组），您希望系统命中指定的 times（num_times x 1 数组），PiecewisePolynomial.CubicShapePreserving 是构建轨迹的合理选择。

desired_position_trajectory = PiecewisePolynomial.CubicShapePreserving(times,positions)
desired_state_source = TrajectorySource(desired_position_trajectory,output_derivative_order=1)
builder.AddSystem(desired_state_source)

output_derivative_order=1 参数使 desired_state_source 输出一个 [位置，速度] 向量而不仅仅是一个位置向量。您可以将 desired_state_source 直接连接到控制器，无需内插器。

通过此设置，您可以将模拟一直推进到 duration，而无需 while 循环。