Change pose cost computation to use the sum of squared l2-norms to reflect the change in trajopt
Do trajectory optimization in between mini segments and add visualization of the desired poses at the gripping points