如何解决逆动力学控制器 Drake
我目前在我的工厂中有一个对象的所需位置与时间的数组。我正在使用逆动力学控制器来将对象驱动到这个所需的位置,但我遇到了一些困难。这是我的做法:
- 我创建了控制器系统
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)
- 我得到了控制器输入和输出端口
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)
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)
- 我将控制器连接到工厂
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))
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
解决方法
- 我添加了一个常量状态源(可能做错了)和一个状态插值器
正如您所怀疑的那样,如果您已经有了希望系统跟踪的位置和时间序列,这不是最好的方法。相反,您应该使用 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 循环。
版权声明:本文内容由互联网用户自发贡献,该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 dio@foxmail.com 举报,一经查实,本站将立刻删除。