In Chapter 7 of Russ Tedrake's Robot Manipulation course, there is a linked notebook about kinematic trajectory optimization with collision avoidance. In that notebook, optimization problems are posed which follow the same pattern.
q0 = plant.GetPositions(plant_context)
...
trajopt.AddPathPositionConstraint(start_constraint, 0)
prog.AddQuadraticErrorCost(
np.eye(num_q), q0, trajopt.control_points()[:, 0]
)
...
trajopt.AddPathPositionConstraint(goal_constraint, 1)
prog.AddQuadraticErrorCost(
np.eye(num_q), q0, trajopt.control_points()[:, -1]
)
I understand this to mean that we want both the final control point, and the initial control point, to be close to the current robot position q0. What is the point of this cost term, given that the initial and final poses are constrained by the start and end constraint? Futhermore, what is the intuition behind this cost term? Why should I want the start and end configuration to be similar to the current configuration?
I'm not sure if this is right, but I see now that the start and end constraints only constrain position, but not orientation. They are specified like start_constraint = PositionConstraint(...). In that case, this cost term may be about making sure that the trajectory starts and ends at an orientation that's close to one specified by the robot's current configuration.