Direct Collocation重建轨迹在采样点间出现振荡的问题求助

我在使用DirectCollocation类的时候，经常得到带有大幅振荡的轨迹解（有时是位置振荡，几乎总是速度振荡）。这些振荡似乎出现在采样点之间，所以我怀疑是中间配点的动力学约束导致的，但一直不知道接下来该怎么排查。

这些振荡主要在关节通过<dynamics damping="0.1"/>添加阻尼后出现，但如果去掉阻尼，又会出现其他问题和痉挛性的运动行为。

我整理了一个最小复现示例，已经删减了原本计划使用的部分约束——通常我还会约束末端执行器笛卡尔速度、关节位置和速度限制、输入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()}')

备注：内容来源于stack exchange，提问作者louis