use angleAverage in pelvis motion control block
logic improvelent (handles case when some link_constraints have dtraj, but others don't)
adding cpp angleAverage function
pelvis cpp controller uses angleAverage
fixed variable not defined bug
adding use_mex=2 to body/pelvis motion controllers
adding a walking test mex unit test