def set_position(joint, goal, t=3.0):
c.set_time(t)
time.sleep(0.1)
c.set_position(goal)
time.sleep(t)
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')