def initiate(self):
rospy.loginfo('start initiate')
currentor.modes = [currentor.mode_non]*currentor.joint_size
currentor.set_control_modes(currentor.modes)
currentor.torque = currentor.set_torque(21, 0)
time.sleep(0.1)
currentor.torque = currentor.set_torque(22, 0)
time.sleep(0.1)
currentor.positions[self.id_map['neck1']] = currentor.set_position(self.id_map['neck1'], self.initial_pose['neck1'])
time.sleep(0.1)
currentor.positions[self.id_map['neck2']] = currentor.set_position(self.id_map['neck2'], self.initial_pose['neck2'])
time.sleep(0.1)
currentor.positions[self.id_map['neck3']] = currentor.set_position(self.id_map['neck3'], self.initial_pose['neck3'])
time.sleep(1)
currentor.positions[self.id_map['hip2']] = currentor.set_position(self.id_map['hip2'], self.initial_pose['hip2'])
time.sleep(0.1)
currentor.positions[self.id_map['hip3']] = currentor.set_position(self.id_map['hip3'], self.initial_pose['hip3'])
time.sleep(1)
currentor.positions[self.id_map['arm_r_joint1']] = currentor.set_position(self.id_map['arm_r_joint1'], self.initial_pose['arm_r_joint1'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_r_joint2']] = currentor.set_position(self.id_map['arm_r_joint2'], self.initial_pose['arm_r_joint2'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_r_joint3']] = currentor.set_position(self.id_map['arm_r_joint3'], self.initial_pose['arm_r_joint3'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_r_joint4']] = currentor.set_position(self.id_map['arm_r_joint4'], self.initial_pose['arm_r_joint4'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_r_joint5']] = currentor.set_position(self.id_map['arm_r_joint5'], self.initial_pose['arm_r_joint5'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_r_joint6']] = currentor.set_position(self.id_map['arm_r_joint6'], self.initial_pose['arm_r_joint6'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_r_joint7']] = currentor.set_position(self.id_map['arm_r_joint7'], self.initial_pose['arm_r_joint7'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_l_joint1']] = currentor.set_position(self.id_map['arm_l_joint1'], self.initial_pose['arm_l_joint1'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_l_joint2']] = currentor.set_position(self.id_map['arm_l_joint2'], self.initial_pose['arm_l_joint2'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_l_joint3']] = currentor.set_position(self.id_map['arm_l_joint3'], self.initial_pose['arm_l_joint3'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_l_joint4']] = currentor.set_position(self.id_map['arm_l_joint4'], self.initial_pose['arm_l_joint4'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_l_joint5']] = currentor.set_position(self.id_map['arm_l_joint5'], self.initial_pose['arm_l_joint5'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_l_joint6']] = currentor.set_position(self.id_map['arm_l_joint6'], self.initial_pose['arm_l_joint6'])
time.sleep(0.1)
currentor.positions[self.id_map['arm_l_joint7']] = currentor.set_position(self.id_map['arm_l_joint7'], self.initial_pose['arm_l_joint7'])
time.sleep(0.1)
currentor.positions[self.id_map['body']] = currentor.set_position(self.id_map['body'], self.initial_pose['body'])
time.sleep(0.1)
rospy.loginfo('done initiate')
def set_torque(joint, goal, t=3.0):
c.set_time(t)
time.sleep(0.1)
c.set_torque(goal)
time.sleep(t)
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')
