generate WorldPositionConstraint and WorldQuatConstraint at the end of dynamic force planning, those constraints will be used in the next step