class Body: def __init__(self, torso_center, lower_body_flag): self._body_center = torso_center self._lower_body_flag = lower_body_flag self.head = Head( Point(torso_center.x, torso_center.y + 15, torso_center.z)) self.torso = Torso( Point(torso_center.x, torso_center.y, torso_center.z)) self.left_arm = Arm( Point(torso_center.x + 6.6, torso_center.y + 8, torso_center.z)) self.right_arm = Arm( Point(torso_center.x - 6.6, torso_center.y + 8, torso_center.z)) self.right_arm.set_slocal_rotate_angle(180, 0, 0) if self._lower_body_flag: self.left_leg = Leg( Point(torso_center.x + 4, torso_center.y - 10.6, torso_center.z)) self.right_leg = Leg( Point(torso_center.x - 4, torso_center.y - 10.6, torso_center.z)) self.right_leg.set_hlocal_rotate_angle(180, 0, 0) def horiz_move(self, gx, gy, gz): movement = tf.translate(Vector( gx, gy, gz)) # Affine matrix for horizontal movement. self._body_center = movement * self._init_torso_center self.head.horiz_move(x, y, z) self.torso.horiz_move(x, y, z) self.left_arm.horiz_move(x, y, z) self.right_arm.horiz_move(x, y, z) if self._lower_body_flag: self.left_leg.horiz_move(x, y, z) self.right_leg.horiz_move(x, y, z) def calc_body_center(self): return self._body_center def get_property_list(self): property_list = [] property_list.extend(self.head.get_property_list()) property_list.extend(self.torso.get_property_list()) property_list.extend(self.left_arm.get_property_list()) property_list.extend(self.right_arm.get_property_list()) if self._lower_body_flag: property_list.extend(self.left_leg.get_property_list()) property_list.extend(self.right_leg.get_property_list()) return property_list def constr_json_data(self): # Construcat JSON data json_data_head = self.head.constr_json_data() json_data_torso = self.torso.constr_json_data() json_data_left_arm = self.left_arm.constr_json_data() json_data_right_arm = self.right_arm.constr_json_data() if self._lower_body_flag: json_data_left_leg = self.left_leg.constr_json_data() json_data_right_leg = self.right_leg.constr_json_data() json_data = { 'joint_angle': { 'left_shoulder': json_data_left_arm['joint_angle']['shoulder'], 'left_elbow': json_data_left_arm['joint_angle']['elbow'], 'right_shoulder': json_data_right_arm['joint_angle']['shoulder'], 'right_elbow': json_data_right_arm['joint_angle']['elbow'] }, 'true_position': { 'head': json_data_head['true_position']['head'], 'torso': json_data_torso['true_position']['torso'], 'left_shouler': json_data_left_arm['true_position']['shoulder'], 'left_elbow': json_data_left_arm['true_position']['elbow'], 'left_hand': json_data_left_arm['true_position']['hand'], 'right_shoulder': json_data_right_arm['true_position']['shoulder'], 'right_elbow': json_data_right_arm['true_position']['elbow'], 'right_hand': json_data_right_arm['true_position']['hand'] } } if self._lower_body_flag: json_data['joint_angle']['left_hip'] = json_data_left_leg[ 'joint_angle']['hip'] json_data['joint_angle']['left_knee'] = json_data_left_leg[ 'joint_angle']['knee'] json_data['joint_angle']['right_hip'] = json_data_right_leg[ 'joint_angle']['hip'] json_data['joint_angle']['right_knee'] = json_data_right_leg[ 'joint_angle']['knee'] json_data['true_position']['left_hip'] = json_data_left_leg[ 'true_position']['hip'] json_data['true_position']['left_knee'] = json_data_left_leg[ 'true_position']['knee'] json_data['true_position']['left_foot'] = json_data_left_leg[ 'true_position']['foot'] json_data['true_position']['right_hip'] = json_data_right_leg[ 'true_position']['hip'] json_data['true_position']['right_knee'] = json_data_right_leg[ 'true_position']['knee'] json_data['true_position']['right_foot'] = json_data_right_leg[ 'true_position']['foot'] return json_data def set_from_json_data(self, json_data): # Set angle parameters from json data (not json file!) self.left_arm.set_from_json_data( json_data['joint_angle']['left_shoulder'], json_data['joint_angle']['left_elbow']) self.right_arm.set_from_json_data( json_data['joint_angle']['right_shoulder'], json_data['joint_angle']['right_elbow']) if self._lower_body_flag: self.left_leg.set_from_json_data( json_data['joint_angle']['left_hip'], json_data['joint_angle']['left_knee']) self.right_leg.set_from_json_data( json_data['joint_angle']['right_hip'], json_data['joint_angle']['right_knee']) def is_collision_body_parts(self): def collision_detection_generator(): # left arm vs. cranicm yield self.left_arm.is_collision_to_sphere( self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # left arm vs. torso yield self.left_arm.is_collision_to_capsule( self.torso.calc_torso_upper_point(), self.torso.calc_torso_lower_point(), self.torso.get_torso_radius()) # left arm vs. right upperarm yield self.left_arm.is_collision_to_capsule( self.right_arm.calc_shoulder_point(), self.right_arm.calc_upperarm_lower_point(), self.right_arm.get_upperarm_radius()) # left arm vs. right elbow yield self.left_arm.is_collision_to_sphere( self.right_arm.calc_elbow_point(), self.right_arm.get_elbow_radius()) # left arm vs. right forearm yield self.left_arm.is_collision_to_capsule( self.right_arm.calc_forearm_upper_point(), self.right_arm.calc_hand_point(), self.right_arm.get_forearm_radius()) # right arm vs. cranicm yield self.right_arm.is_collision_to_sphere( self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # right arm vs. torso yield self.right_arm.is_collision_to_capsule( self.torso.calc_torso_upper_point(), self.torso.calc_torso_lower_point(), self.torso.get_torso_radius()) if self._lower_body_flag: # left arm vs. left thigh yield self.left_arm.is_collision_to_capsule( self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius()) # left arm vs. left knee yield self.left_arm.is_collision_to_sphere( self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius()) # left arm vs. left calf yield self.left_arm.is_collision_to_capsule( self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius()) # left arm vs. right thigh yield self.left_arm.is_collision_to_capsule( self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius()) # left arm vs. right knee yield self.left_arm.is_collision_to_sphere( self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # left arm vs. right calf yield self.left_arm.is_collision_to_capsule( self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) # right arm vs. left thigh yield self.right_arm.is_collision_to_capsule( self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius()) # right arm vs. left thigh yield self.right_arm.is_collision_to_capsule( self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius()) # right arm vs. left knee yield self.right_arm.is_collision_to_sphere( self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius()) # right arm vs. left calf yield self.right_arm.is_collision_to_capsule( self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius()) # right arm vs. right thigh yield self.right_arm.is_collision_to_capsule( self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius()) # right arm vs. right knee yield self.right_arm.is_collision_to_sphere( self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # right arm vs. right calf yield self.right_arm.is_collision_to_capsule( self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) # left leg vs. cranicm yield self.left_leg.is_collision_to_sphere( self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # left leg vs. right thigh yield self.left_leg.is_collision_to_capsule( self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius()) # left leg vs. right knee yield self.left_leg.is_collision_to_sphere( self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # left leg vs. right calf yield self.left_leg.is_collision_to_capsule( self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) # right leg vs. cranicm yield self.right_leg.is_collision_to_sphere( self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # torso vs. left knee yield self.torso.is_collision_to_sphere( self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius()) # torso vs. left calf yield self.torso.is_collision_to_capsule( self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius()) # torso vs. right knee yield self.torso.is_collision_to_sphere( self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # torso vs. right calf yield self.torso.is_collision_to_capsule( self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) for detection in collision_detection_generator(): if detection: return True return False
class Body: def __init__(self, torso_center, lower_body_flag): self._body_center = torso_center self._lower_body_flag = lower_body_flag self.head = Head(Point(torso_center.x, torso_center.y+15, torso_center.z)) self.torso = Torso(Point(torso_center.x, torso_center.y, torso_center.z)) self.left_arm = Arm(Point(torso_center.x+6.6, torso_center.y+8, torso_center.z)) self.right_arm = Arm(Point(torso_center.x-6.6, torso_center.y+8, torso_center.z)) self.right_arm.set_slocal_rotate_angle(180, 0, 0) if self._lower_body_flag: self.left_leg = Leg(Point(torso_center.x+4, torso_center.y-10.6, torso_center.z)) self.right_leg = Leg(Point(torso_center.x-4, torso_center.y-10.6, torso_center.z)) self.right_leg.set_hlocal_rotate_angle(180, 0, 0) def horiz_move(self, gx, gy, gz): movement = tf.translate(Vector(gx, gy, gz)) # Affine matrix for horizontal movement. self._body_center = movement * self._init_torso_center self.head.horiz_move(x, y, z) self.torso.horiz_move(x, y, z) self.left_arm.horiz_move(x, y, z) self.right_arm.horiz_move(x, y, z) if self._lower_body_flag: self.left_leg.horiz_move(x, y, z) self.right_leg.horiz_move(x, y, z) def calc_body_center(self): return self._body_center def get_property_list(self): property_list = [] property_list.extend(self.head.get_property_list()) property_list.extend(self.torso.get_property_list()) property_list.extend(self.left_arm.get_property_list()) property_list.extend(self.right_arm.get_property_list()) if self._lower_body_flag: property_list.extend(self.left_leg.get_property_list()) property_list.extend(self.right_leg.get_property_list()) return property_list def constr_json_data(self): # Construcat JSON data json_data_head = self.head.constr_json_data() json_data_torso = self.torso.constr_json_data() json_data_left_arm = self.left_arm.constr_json_data() json_data_right_arm = self.right_arm.constr_json_data() if self._lower_body_flag: json_data_left_leg = self.left_leg.constr_json_data() json_data_right_leg= self.right_leg.constr_json_data() json_data = { 'joint_angle': { 'left_shoulder': json_data_left_arm['joint_angle']['shoulder'], 'left_elbow': json_data_left_arm['joint_angle']['elbow'], 'right_shoulder': json_data_right_arm['joint_angle']['shoulder'], 'right_elbow': json_data_right_arm['joint_angle']['elbow'] }, 'true_position': { 'head': json_data_head['true_position']['head'], 'torso': json_data_torso['true_position']['torso'], 'left_shouler': json_data_left_arm['true_position']['shoulder'], 'left_elbow': json_data_left_arm['true_position']['elbow'], 'left_hand': json_data_left_arm['true_position']['hand'], 'right_shoulder': json_data_right_arm['true_position']['shoulder'], 'right_elbow': json_data_right_arm['true_position']['elbow'], 'right_hand': json_data_right_arm['true_position']['hand'] } } if self._lower_body_flag: json_data['joint_angle']['left_hip'] = json_data_left_leg['joint_angle']['hip'] json_data['joint_angle']['left_knee'] = json_data_left_leg['joint_angle']['knee'] json_data['joint_angle']['right_hip'] = json_data_right_leg['joint_angle']['hip'] json_data['joint_angle']['right_knee'] = json_data_right_leg['joint_angle']['knee'] json_data['true_position']['left_hip'] = json_data_left_leg['true_position']['hip'] json_data['true_position']['left_knee'] = json_data_left_leg['true_position']['knee'] json_data['true_position']['left_foot'] = json_data_left_leg['true_position']['foot'] json_data['true_position']['right_hip'] = json_data_right_leg['true_position']['hip'] json_data['true_position']['right_knee'] = json_data_right_leg['true_position']['knee'] json_data['true_position']['right_foot'] = json_data_right_leg['true_position']['foot'] return json_data def set_from_json_data(self, json_data): # Set angle parameters from json data (not json file!) self.left_arm.set_from_json_data(json_data['joint_angle']['left_shoulder'], json_data['joint_angle']['left_elbow']) self.right_arm.set_from_json_data(json_data['joint_angle']['right_shoulder'], json_data['joint_angle']['right_elbow']) if self._lower_body_flag: self.left_leg.set_from_json_data(json_data['joint_angle']['left_hip'], json_data['joint_angle']['left_knee']) self.right_leg.set_from_json_data(json_data['joint_angle']['right_hip'], json_data['joint_angle']['right_knee']) def is_collision_body_parts(self): def collision_detection_generator(): # left arm vs. cranicm yield self.left_arm.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # left arm vs. torso yield self.left_arm.is_collision_to_capsule(self.torso.calc_torso_upper_point(), self.torso.calc_torso_lower_point(), self.torso.get_torso_radius()) # left arm vs. right upperarm yield self.left_arm.is_collision_to_capsule(self.right_arm.calc_shoulder_point(), self.right_arm.calc_upperarm_lower_point(), self.right_arm.get_upperarm_radius()) # left arm vs. right elbow yield self.left_arm.is_collision_to_sphere(self.right_arm.calc_elbow_point(), self.right_arm.get_elbow_radius()) # left arm vs. right forearm yield self.left_arm.is_collision_to_capsule(self.right_arm.calc_forearm_upper_point(), self.right_arm.calc_hand_point(), self.right_arm.get_forearm_radius()) # right arm vs. cranicm yield self.right_arm.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # right arm vs. torso yield self.right_arm.is_collision_to_capsule(self.torso.calc_torso_upper_point(), self.torso.calc_torso_lower_point(), self.torso.get_torso_radius()) if self._lower_body_flag: # left arm vs. left thigh yield self.left_arm.is_collision_to_capsule(self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius()) # left arm vs. left knee yield self.left_arm.is_collision_to_sphere(self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius()) # left arm vs. left calf yield self.left_arm.is_collision_to_capsule(self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius()) # left arm vs. right thigh yield self.left_arm.is_collision_to_capsule(self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius()) # left arm vs. right knee yield self.left_arm.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # left arm vs. right calf yield self.left_arm.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) # right arm vs. left thigh yield self.right_arm.is_collision_to_capsule(self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius()) # right arm vs. left thigh yield self.right_arm.is_collision_to_capsule(self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius()) # right arm vs. left knee yield self.right_arm.is_collision_to_sphere(self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius()) # right arm vs. left calf yield self.right_arm.is_collision_to_capsule(self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius()) # right arm vs. right thigh yield self.right_arm.is_collision_to_capsule(self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius()) # right arm vs. right knee yield self.right_arm.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # right arm vs. right calf yield self.right_arm.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) # left leg vs. cranicm yield self.left_leg.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # left leg vs. right thigh yield self.left_leg.is_collision_to_capsule(self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius()) # left leg vs. right knee yield self.left_leg.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # left leg vs. right calf yield self.left_leg.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) # right leg vs. cranicm yield self.right_leg.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius()) # torso vs. left knee yield self.torso.is_collision_to_sphere(self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius()) # torso vs. left calf yield self.torso.is_collision_to_capsule(self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius()) # torso vs. right knee yield self.torso.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius()) # torso vs. right calf yield self.torso.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius()) for detection in collision_detection_generator(): if detection: return True return False