I am working with the DirectCollocation class and an often getting trajectory solutions with large oscillations (sometimes in the position, almost always in the velocity). These oscillations seem to happen between the sample points, so I was thinking it may be due to the dynamics constraints applied at the intermediate collocation points, but I've had trouble figuring out where to go from here.
These oscillations seem to appear mainly when the joints have dampening applied via <dynamics damping="0.1"/>, but without dampening, other problems and spastic behaviors occur.
Here is the code: https://github.com/louietouie/oscillating_ping_pong/tree/main. I've cut down on the constraints I plan to use (I usually also constrain end-effector cartesian velocity, joint position and velocity limits, u limits, but cut out these to create this minimum example). The constraints in the sample force the robot to start at joint position (0,0,0) and move to joint position (1,1,1), starting and ending at 0 velocity.
Things I've tried:
New costs penalizing the squared difference between x_dot at adjacent sample points. I believe this failed because the problem isn't with velocity changes between sample points, but between them.
Constraints around joint limits and velocity limits, which sometimes partially hide the problem
Increasing the mass / length of the final link has helped, but then it no longer reflects the true robot.
FirstOrderHold initial guesses similar to the swing up example
More sample points (just makes the oscillations faster)
Does anyone have advice on things worth trying out next to remove these oscillations from the reconstructed trajectory? Thank you!
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()}')
