I have a path with values [time, joint_angle1, joint_angle2, joint_angle1_velocity, joint_angle2_velocity] that I want my robot arm to follow. At t = 0, arm's state is not on the path and I want to create a trajectory that moves the robot arm on to my path as quickly as possible.
I currently have direct collocation algorithm working such that it returns the quickest trajectory to point [joint_angle1, joint_angle2, joint_angle1_velocity, joint_angle2_velocity].
However, I need a trajectory that converges to my desired path as quickly as possible. My current solution is to run the trajectory optimizer on multiple points on path and take the earliest point that returns a trajectory. However, this requires me to run direct collocation algorithm multiple times, whick can be slow.
Do you have advice as to how to modify my direct collocation implementation to allow me to converge to a path? I've tried few things, but haven't had much luck with it.
Few things I tried:
- add path to state and tried to have the final points path and trajectory to be the same. This ended up slowing solver down a ton because it increased the state variable size.
- I've tried adding error from path at each knot point to cost, but that doesn't really seem to do much.
Is there a concept of optional constraint?
Any help would be appreciated.