Changed PD gains of impedance for arms and torso, tested in simulation