00001 from math import radians 00002 00003 port = '/dev/ttyUSB1' 00004 00005 servo_param = { 00006 1: {'name': 'head_pan_joint', 00007 'home_encoder': 512, 00008 'max_speed': radians(180), 00009 'max_ang': radians(145.), 00010 'min_ang': radians(-145.) 00011 }, 00012 2: {'name': 'head_tilt_joint', 00013 'home_encoder': 512, 00014 'max_speed': radians(180), 00015 'max_ang': radians(145.), 00016 'min_ang': radians(-145.) 00017 00018 }, 00019 3: {'name': 'right_shoulder_pan_joint', 00020 'home_encoder': 512, 00021 'max_speed': radians(180), 00022 'max_ang': radians(360.), 00023 'min_ang': radians(-360.) 00024 }, 00025 4: {'name': 'right_shoulder_lift_joint', 00026 'home_encoder': 512, 00027 'max_speed': radians(180), 00028 'max_ang': radians(360.), 00029 'min_ang': radians(-360.) 00030 }, 00031 5: {'name': 'right_arm_roll_joint', 00032 'home_encoder': 512, 00033 'max_speed': radians(180), 00034 'max_ang': radians(360.), 00035 'min_ang': radians(-360.) 00036 }, 00037 6: {'name': 'right_wrist_joint', 00038 'home_encoder': 512, 00039 'max_speed': radians(180), 00040 'max_ang': radians(360.), 00041 'min_ang': radians(-360.), 00042 'flipped': True 00043 }, 00044 7: {'name': 'left_shoulder_pan_joint', 00045 'home_encoder': 512, 00046 'max_speed': radians(180), 00047 'max_ang': radians(360.), 00048 'min_ang': radians(-360.) 00049 }, 00050 8: {'name': 'left_shoulder_lift_joint', 00051 'home_encoder': 512, 00052 'max_speed': radians(180), 00053 'max_ang': radians(360.), 00054 'min_ang': radians(-360.) 00055 }, 00056 9: {'name': 'left_arm_roll_joint', 00057 'home_encoder': 512, 00058 'max_speed': radians(180), 00059 'max_ang': radians(360.), 00060 'min_ang': radians(-360.) 00061 }, 00062 10: {'name': 'left_wrist_joint', 00063 'home_encoder': 512, 00064 'max_speed': radians(180), 00065 'max_ang': radians(360.), 00066 'min_ang': radians(-360.), 00067 'flipped': True 00068 }, 00069 11: {'name': 'torso_joint', 00070 'home_encoder': 512, 00071 'max_speed': radians(180), 00072 'max_ang': radians(360.), 00073 'min_ang': radians(-360.) 00074 }, 00075 12: {'name': 'left_elbow_joint', 00076 'home_encoder': 512, 00077 'max_speed': radians(180), 00078 'max_ang': radians(360.), 00079 'min_ang': radians(-360.) 00080 }, 00081 13: {'name': 'right_elbow_joint', 00082 'home_encoder': 512, 00083 'max_speed': radians(180), 00084 'max_ang': radians(360.), 00085 'min_ang': radians(-360.) 00086 } 00087 } 00088