def set_pid_gain(joint, p=0.0, i=0.0, d=0.0):
c.set_pid_gain(joint, p, i, d)
time.sleep(0.1)
def send_zero(self):
rospy.loginfo('start send zero')
currentor.set_pid_gain(self.id_map['neck1'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_pid_gain(self.id_map['neck2'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_pid_gain(self.id_map['neck3'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_pid_gain(self.id_map['arm_r_joint1'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_pid_gain(self.id_map['arm_r_joint2'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_pid_gain(self.id_map['arm_r_joint3'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_pid_gain(self.id_map['arm_r_joint5'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_pid_gain(self.id_map['arm_r_joint7'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_pid_gain(self.id_map['arm_l_joint1'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_pid_gain(self.id_map['arm_l_joint2'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_pid_gain(self.id_map['arm_l_joint3'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_pid_gain(self.id_map['arm_l_joint5'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_pid_gain(self.id_map['arm_l_joint7'], 0.0, 0.0, 10.0)
time.sleep(3)
currentor.set_interpolate(self.id_map['body'], 'interpolatePIDLinear', 5.0)
currentor.set_pid_gain(self.id_map['body'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_interpolate(self.id_map['hip2'], 'interpolatePIDLinear', 5.0)
currentor.set_pid_gain(self.id_map['hip2'], 0.0, 0.0, 10.0)
time.sleep(0.1)
currentor.set_interpolate(self.id_map['hip3'], 'interpolatePIDLinear', 5.0)
currentor.set_pid_gain(self.id_map['hip3'], 0.0, 0.0, 10.0)
time.sleep(5)
currentor.set_interpolate(self.id_map['arm_r_joint4'], 'interpolatePIDLinear', 1.0)
currentor.set_pid_gain(self.id_map['arm_r_joint4'], 0.0, 0.0, 0.0)
time.sleep(0.1)
currentor.set_interpolate(self.id_map['arm_r_joint6'], 'interpolatePIDLinear', 1.0)
currentor.set_pid_gain(self.id_map['arm_r_joint6'], 0.0, 0.0, 0.0)
time.sleep(0.1)
currentor.set_interpolate(self.id_map['arm_l_joint4'], 'interpolatePIDLinear', 1.0)
currentor.set_pid_gain(self.id_map['arm_l_joint4'], 0.0, 0.0, 0.0)
time.sleep(0.1)
currentor.set_interpolate(self.id_map['arm_l_joint6'], 'interpolatePIDLinear', 1.0)
currentor.set_pid_gain(self.id_map['arm_l_joint6'], 0.0, 0.0, 0.0)
time.sleep(0.1)
currentor.original_mode()
currentor.set_torque(self.id_map['neck1'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['neck2'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['neck3'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_r_joint1'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_r_joint2'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_r_joint3'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_r_joint4'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_r_joint5'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_r_joint6'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_r_joint7'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_l_joint1'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_l_joint2'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_l_joint3'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_l_joint4'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_l_joint5'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_l_joint6'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['arm_l_joint7'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['body'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['hip2'], 0.0)
time.sleep(0.1)
currentor.set_torque(self.id_map['hip3'], 0.0)
time.sleep(0.1)
time.sleep(3)
rospy.loginfo('done send zero')