Fixed bug in soft hand. Updated PIDs arm