Multi-robot formation control and object transport in dynamic environments via constrained optimization