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')