Прямая коллокация Осциллирующая реконструированная траектория состояния ⇐ Python

[Anonymous]

Я работаю с классом DirectCollocation и часто получаю решения по траектории с большими колебаниями (иногда по положению, почти всегда по скорости). Кажется, эти колебания происходят между точками выборки, поэтому я подумал, что это может быть связано с динамическими ограничениями, наложенными в промежуточных точках сочетания, но мне было трудно понять, куда двигаться дальше.
Эти колебания появляются в основном, когда к суставам применяется демпфирование с помощью , но без демпфирования возникают другие проблемы и спастическое поведение.
Вот код: https://github.com/louietouie/oscillati ... /tree/main. Я сократил ограничения, которые планирую использовать (обычно я также ограничиваю декартову скорость конечного рабочего органа, положение сустава и пределы скорости, ограничения u, но вырезаю их, чтобы создать этот минимальный пример). Ограничения в примере вынуждают робота начинать с позиции соединения (0,0,0) и перемещаться в позицию соединения (1,1,1), начиная и заканчивая со скоростью 0.
Что я пробовал:

• Новые затраты, наказывающие квадрат разницы между x_dot в соседних точках выборки. Я считаю, что это не удалось, потому что проблема заключается не в изменении скорости между точками выборки, а между ними.
• Ограничения вокруг пределов соединений и пределов скорости, которые иногда частично скрывают проблему.
• Увеличение массы/длины конечного звена помогло, но тогда оно больше не отражает истинного робота.
• FirstOrderHold исходные предположения аналогичны примеру с подъемом
• Больше точек выборки (просто ускоряет колебания)

Есть ли у кого-нибудь совет о том, что стоит попробовать дальше, чтобы убрать эти колебания из реконструированной траектории? Спасибо!




from pydrake.all import (
Rgba,
StartMeshcat,
Sphere,
DiagramBuilder,
AddMultibodyPlantSceneGraph,
Parser,
DirectCollocation,
Solve,
RigidTransform,
MeshcatVisualizer,
MeshcatVisualizerParams,
Role,
PiecewisePolynomial,
SnoptSolver,
ApplyVisualizationConfig, VisualizationConfig, ContactModel
)
import xacro
import numpy as np

meshcat = StartMeshcat()
meshcat.Delete()

print("BUILDING SCENE")
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
parser = Parser(plant, scene_graph)
urdf = xacro.process_file("models/robot.urdf.xacro").toprettyxml(indent=' ')
(robot,) = parser.AddModelsFromString(urdf, ".urdf")
basebody = plant.GetBodyByName("base_link")
plant.WeldFrames(plant.world_frame(), basebody.body_frame(), RigidTransform([.1,.7,0]))
plant.set_contact_model(ContactModel.kPoint)
plant.Finalize()

print("CREATING VISUALIZER")
visualizer_config = VisualizationConfig()
visualizer_config.publish_illustration = True
visualizer_config.publish_inertia = True
ApplyVisualizationConfig(visualizer_config, builder, plant=plant, scene_graph=scene_graph, meshcat=meshcat)

print("CREATING CONTEXTS")
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context_clone = plant.GetMyContextFromRoot(context)
plant_context = plant.CreateDefaultContext()

print("PLANT FINALIZED")
NUM_JOINTS = plant.get_actuation_input_port().size()

print("DIRECT COLLOCATION")
num_samples = 11
time_constraint = 1

direct_collocation = DirectCollocation(
plant,
plant_context,
num_time_samples=num_samples,
input_port_index=plant.get_actuation_input_port().get_index(),
minimum_time_step=time_constraint/num_samples * 0.9,
maximum_time_step=time_constraint/num_samples * 1.1,
)
prog = direct_collocation.prog()

print("DEFINING COST")
R = np.diag([1, 2, 5])
u = direct_collocation.input()
direct_collocation.AddRunningCost(u.T @ R @ u)

print("DEFINE CONSTRAINTS")
direct_collocation.AddEqualTimeIntervalsConstraints()
direct_collocation.AddDurationBounds(time_constraint, time_constraint)

start_state = np.zeros(NUM_JOINTS*2)
end_state = np.concatenate([np.ones(NUM_JOINTS), np.zeros(NUM_JOINTS)])
prog.AddBoundingBoxConstraint(
start_state, start_state, direct_collocation.initial_state())
prog.AddBoundingBoxConstraint(
end_state, end_state, direct_collocation.final_state())

print("SOLVE")
solver = SnoptSolver()
solver_id = solver.solver_id()
major_tol = 1e-3
minor_tol = 1e-3
prog.SetSolverOption(solver_id, "Feasibility tolerance", major_tol)
prog.SetSolverOption(solver_id, "Major feasibility tolerance", major_tol)
prog.SetSolverOption(solver_id, "Major optimality tolerance", major_tol)
prog.SetSolverOption(solver_id, "Minor feasibility tolerance", minor_tol)
prog.SetSolverOption(solver_id, "Minor optimality tolerance", minor_tol)

result = Solve(prog)
print(f'success: {result.is_success()}')


Подробнее здесь: https://stackoverflow.com/questions/798 ... trajectory