Initial commit of motoman demo manipulation