Remove line "dxpdxm(2,6)=1;" which adds a bogus dimension to the jacobian dxpdxm (should have dim [4,5])
Remove line "dxpdxm(2,6)=1;" which adds a bogus dimension to the jacobian dxpdxm (should have dim [4,5])

billhoffman/drake,billhoffman/drake,psiorx/drake,psiorx/drake,psiorx/drake,psiorx/drake,sheim/drake,sheim/drake,sheim/drake,billhoffman/drake,billhoffman/drake,sheim/drake,billhoffman/drake,psiorx/drake,sheim/drake,sheim/drake,billhoffman/drake,sheim/drake,sheim/drake,billhoffman/drake,billhoffman/drake,psiorx/drake,psiorx/drake,psiorx/drake