def main(): with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(101, sess) output_stride = model_cfg['output_stride'] scale_factor = 0.7125 last_res = 5 cap = cv2.VideoCapture(0) cap.set(3, 640) cap.set(4, 480) eggNet = egg_model.PandaEgg() eggNet.load_weights('data/egg_model_weights.csv') while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=1, min_pose_score=0.15) keypoint_coords *= output_scale overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) if np.array_equal(keypoint_coords, np.zeros((1, 17, 2))): text = 'Nope' else: res = eggNet.pose_detect(keypoint_coords) if res != last_res: if res == 0: sio.emit('message', '0') text = 'STANDING' last_res = res elif res == 1: sio.emit('message', '1') text = 'SITTING' last_res = res elif res == 2: sio.emit('message', '2') text = 'LYING' last_res = res cv2.putText(overlay_image, text, (0, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 2) cv2.imshow('posenet', overlay_image) if cv2.waitKey(1) & 0xFF == 27: break
def main(): with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(argModel, sess) output_stride = model_cfg['output_stride'] cap = cv2.VideoCapture(cam_id) cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) cap.set(3, cam_width) cap.set(4, cam_height) while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) leftHandHigh = False leftWrist_h = keypoint_coords[0, :, :][9][0] leftShoulder_h = keypoint_coords[0, :, :][5][0] if (leftWrist_h < leftShoulder_h): leftHandHigh = True rightHandHigh = False rightWrist_h = keypoint_coords[0, :, :][10][0] rightShoulder_h = keypoint_coords[0, :, :][6][0] if (rightWrist_h < rightShoulder_h): rightHandHigh = True if (rightHandHigh and leftHandHigh): pos = 'U_arm' elif (rightHandHigh): pos = 'R_arm' elif (leftHandHigh): pos = 'L_arm' else: pos = 'D_arm' background[200:200 + 346, 200:200 + 240] = imcollection[pos] cv2.imshow('posenet', background) if cv2.waitKey(1) & 0xFF == ord('q'): break
def get_tilt(self, visualize=False, debug=False): ''' Returns: 'left', 'right', or '' ''' input_image, display_image, output_scale = posenet.read_cap( self.cap, scale_factor=args.scale_factor, output_stride=self.output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = self.tf_session.run( self.model_outputs, feed_dict={'image:0': input_image}) pose_scores, self.keypoint_scores, self.keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=self.output_stride, max_pose_detections=10, min_pose_score=0.15) self.keypoint_coords *= output_scale pose_index = self.get_principal_index() if self.frame_count < 5: self.angle_baseline += self.get_angle(pose_index) self.frame_count += 1 elif self.frame_count == 5: self._calibrate() self.frame_count += 1 is_turned = self.is_turned(pose_index) if self.get_angle( pose_index ) > self.tilt_threshold + self.angle_baseline and not is_turned: tilt = 'left' elif self.get_angle( pose_index ) < -1 * self.tilt_threshold + self.angle_baseline and not is_turned: tilt = 'right' else: tilt = '' if debug: if tilt: print('{} tilt detected!'.format(tilt)) else: print('.') print() if visualize: # TODO this isn't particularly fast, use GL for drawing and display someday... overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, self.keypoint_scores, self.keypoint_coords, min_pose_score=0.15, min_part_score=0.1) cv2.imshow('posenet', overlay_image) return tilt
def get_poses(self): # Read image input_image, display_image, output_scale = posenet.read_cap( self.cap, scale_factor=self.scale_factor, output_stride=self.output_stride) # Retrieve the heatmaps from the image heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = self.sess.run( self.model_outputs, feed_dict={'image:0': input_image} ) # Decode the heatmaps into poses and keypoints pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=self.output_stride, max_pose_detections=10, min_pose_score=MIN_POSE_SCORE) # Draw the poses onto the image keypoint_coords *= output_scale overlay_image, num_keypoints_detected, people_location, num_of_people_detected = posenet.draw_skel_and_kp( display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=MIN_POSE_SCORE, min_part_score=MIN_KEYPOINT_SCORE) return overlay_image, num_keypoints_detected, people_location, num_of_people_detected
def main(): # D:\Programming-Github\Sem-6\MiniProject\posenet-pytorch-master\posenet\converter\wget.py # go here and find # data = json.loads(response.content) # replace with # data = json.loads(response.content.decode('utf-8')) model = posenet.load_model(args.model) model = model.cuda() output_stride = model.output_stride cap = cv2.VideoCapture( "C:\\Users\\habil\\Pictures\\Camera Roll\\WIN_20200329_15_03_35_Pro.mp4" ) cap.set(3, args.cam_width) cap.set(4, args.cam_height) start = time.time() frame_count = 0 while True: try: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) except OSError: break with torch.no_grad(): input_image = torch.Tensor(input_image).cuda() heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = model( input_image) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multiple_poses( heatmaps_result.squeeze(0), offsets_result.squeeze(0), displacement_fwd_result.squeeze(0), displacement_bwd_result.squeeze(0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale #overlay_image = posenet.draw_skel_and_kp( #display_image, pose_scores, keypoint_scores, keypoint_coords, #min_pose_score=0.15, min_part_score=0.1) print(keypoint_coords) #overlay_image = posenet.draw_skel_and_kp(display_image,[],[],[]) #cv2.imshow('posenet', overlay_image) frame_count += 1 if cv2.waitKey(1) & 0xFF == ord('q'): break print('Average FPS: ', frame_count / (time.time() - start))
def main(): write = open_csv_file('../data/lie.csv', 'w') cam_width = 640 cam_height = 480 scale_factor = 0.7125 with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(101, sess) output_stride = model_cfg['output_stride'] cap = cv2.VideoCapture(0) cap.set(3, cam_width) cap.set(4, cam_height) irr_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor, output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image} ) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=1, min_pose_score=0.15) keypoint_coords *= output_scale overlay_image = posenet.draw_skel_and_kp( display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) cv2.putText(overlay_image, str(irr_count), (0, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 2) cv2.imshow('posenet', overlay_image) keypoint_coords[:, :, 0] = keypoint_coords[:, :, 0] / 480 keypoint_coords[:, :, 1] = keypoint_coords[:, :, 1] / 640 flat_array = keypoint_coords.flatten() new_data = np.insert(flat_array, 0, 0)# Insert the label for training data, stand:0, sit:1, lie:2 to index 0 write.writerow(new_data) irr_count += 1 if cv2.waitKey(1) & 0xFF == 27: break elif irr_count == 1200: break
def main(): with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] if args.file is not None: cap = cv2.VideoCapture(args.file) else: cap = cv2.VideoCapture(args.cam_id) cap.set(3, args.cam_width) cap.set(4, args.cam_height) start = time.time() frame_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image} ) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale overlay_image = posenet.draw_skel_and_kp( display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) cv2.imshow('posenet', overlay_image) frame_count += 1 if cv2.waitKey(1) & 0xFF == ord('q'): break # Open file file = open("log.txt", "a") for pi in range(len(pose_scores)): if pose_scores[pi] == 0.: break print('Pose #%d, score = %f' % (pi, pose_scores[pi])) file.write('Pose #%d, score = %f \n' % (pi, pose_scores[pi])) for ki, (s, c) in enumerate(zip(keypoint_scores[pi, :], keypoint_coords[pi, :, :])): print('Keypoint %s, score = %f, coord = %s' % (posenet.PART_NAMES[ki], s, c)) file.write('Keypoint %s, score = %f, coord = %s \n' % (posenet.PART_NAMES[ki], s, c)) # Close file file.close() print('Average FPS: ', frame_count / (time.time() - start))
def main(): with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(argModel, sess) output_stride = model_cfg['output_stride'] cap = cv2.VideoCapture(cam_id) cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) cap.set(3, cam_width) cap.set(4, cam_height) while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) leftHandHigh = False leftWrist_h = keypoint_coords[0, :, :][9][0] leftShoulder_h = keypoint_coords[0, :, :][5][0] if (leftWrist_h < leftShoulder_h): leftHandHigh = True rightHandHigh = False rightWrist_h = keypoint_coords[0, :, :][10][0] rightShoulder_h = keypoint_coords[0, :, :][6][0] if (rightWrist_h < rightShoulder_h): rightHandHigh = True font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(overlay_image, str(leftHandHigh) + str(rightHandHigh), (10, 450), font, 1, (0, 255, 0), 1, cv2.LINE_AA) cv2.imshow('posenet', overlay_image) if cv2.waitKey(1) & 0xFF == ord('q'): break
def recorded_main_vid(): with tf.compat.v1.Session() as sess: model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] url = "https://www.youtube.com/watch?v=2HTvZp5rPrg&t=7s" video = pafy.new(url) best = video.getbest(preftype="mp4") cap = cv2.VideoCapture() cap.open(best.url) cap.set(cv2.CAP_PROP_FPS, int(30)) start = time.time() frame_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image} ) global keypoint2 pose_scores, keypoint_scores, keypoint_coords2 = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords2 *= output_scale keypoint2 = np.array(keypoint_coords2[0]) # print("camera", np.array(keypoint_coords2[0]).shape) # for ii, score in enumerate(pose_scores): # print("************") # print(ii, "----------", score) # print(pose_scores) overlay_image = posenet.draw_skel_and_kp( display_image, pose_scores, keypoint_scores, keypoint_coords2, min_pose_score=0.15, min_part_score=0.1) # overlay_image = cv2.resize(overlay_image, (0,0), fx=0.8, fy=0.8) ret, jpeg = cv2.imencode('.jpg', overlay_image) frame = jpeg.tobytes() yield (b'--frame\r\n' b'Content-Type: image/jpeg\r\n\r\n' + frame + b'\r\n\r\n')
def main(): cap = cvs.VideoCapture(0) with tf.Session() as sess: print('load models...') model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] start = time.time() frame_count = 0 while True: sleep(30) img = cvs.read() frame_count += 1 if img is None: continue if cam_id > 0: img = cvs.flip(img, 0) input_image, display_image, output_scale = posenet.read_cap( img, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale #print keypoint_coords # TODO this isn't particularly fast, use GL for drawing and display someday... overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) cvs.imshow(overlay_image) frame_count += 1 # global lbs lbs = 'Average FPS: ' + str(frame_count / (time.time() - start)) cvs.setLbs(lbs)
def main(): with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] if args.file is not None: cap = cv2.VideoCapture(args.file) else: cap = cv2.VideoCapture(gstreamer_pipeline(flip_method=2), cv2.CAP_GSTREAMER) #cap.set(3, args.cam_width) #cap.set(4, args.cam_height) start = time.time() frame_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale # TODO this isn't particularly fast, use GL for drawing and display someday... overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) cv2.namedWindow("posenet", cv2.WND_PROP_FULLSCREEN) cv2.setWindowProperty("posenet", cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN) cv2.imshow('posenet', overlay_image) frame_count += 1 if cv2.waitKey(1) & 0xFF == ord('q'): break print('Average FPS: ', frame_count / (time.time() - start))
def main(): stat = '' with tf.compat.v1.Session() as sess: model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] if args.file is not None: cap = cv2.VideoCapture(args.file) else: cap = cv2.VideoCapture(args.cam_id) cap.set(3, args.cam_width) cap.set(4, args.cam_height) start = time.time() frame_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale stat_res = to_http.httpreq(keypoint_coords[0, 5, :], stat) stat = stat_res # TODO this isn't particularly fast, use GL for drawing and display someday... overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) cv2.imshow('posenet', overlay_image) frame_count += 1 if cv2.waitKey(1) & 0xFF == ord('q'): break print('Average FPS: ', frame_count / (time.time() - start))
def gen_frames(): # generate frame by frame from camera with tf.compat.v1.Session() as sess: model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] if args.file is not None: cap = cv2.VideoCapture(args.file) else: cap = cv2.VideoCapture(args.cam_id) cap.set(3, args.cam_width) cap.set(4, args.cam_height) start = time.time() frame_count = 0 while True: # Capture frame-by-frame # success, frame = camera.read() # read the camera frame input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale # TODO this isn't particularly fast, use GL for drawing and display someday... overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) # if not success: # break #else: ret, buffer = cv2.imencode('.jpg', overlay_image) overlay_image = buffer.tobytes() yield (b'--frame\r\n' b'Content-Type: image/jpeg\r\n\r\n' + overlay_image + b'\r\n') # concat frame one by one and show result
def webcam_main_vid(): with tf.compat.v1.Session() as sess: model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] if args.file is not None: cap = cv2.VideoCapture(args.file) # cap=cv2.VideoCapture(1) else: cap = cv2.VideoCapture(args.cam_id) # cap=cv2.VideoCapture(1) start = time.time() frame_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image} ) pose_scores, keypoint_scores, keypoint_coords1 = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) # for ii, score in enumerate(pose_scores): # print("************") # print(ii, "----------", score) global keypoint1 keypoint_coords1 *= output_scale keypoint1 = np.array(keypoint_coords1[0]) overlay_image = posenet.draw_skel_and_kp( display_image, pose_scores, keypoint_scores, keypoint_coords1, min_pose_score=0.15, min_part_score=0.1) # overlay_image = cv2.resize(overlay_image, (0,0), fx=0.8, fy=0.8) ret, jpeg = cv2.imencode('.jpg', overlay_image) frame = jpeg.tobytes() yield (b'--frame\r\n' b'Content-Type: image/jpeg\r\n\r\n' + frame + b'\r\n\r\n')
def _video_loop(self): """ Get frame from the video stream and show it in Tkinter """ input_image, display_image, output_scale = posenet.read_cap( self._vs, scale_factor=PostureAidConfig.config("SCALE_FACTOR"), output_stride=self._output_stride ) with torch.no_grad(): if torch.cuda.is_available(): input_image = torch.Tensor(input_image).cuda() else: input_image = torch.Tensor(input_image) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = self._model(input_image) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multiple_poses( heatmaps_result.squeeze(0), offsets_result.squeeze(0), displacement_fwd_result.squeeze(0), displacement_bwd_result.squeeze(0), output_stride=self._output_stride, max_pose_detections=10, min_pose_score=0.15 ) keypoint_coords *= output_scale current_pos = posenet.get_pos_from_img( display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1 ) if self._running: if not check_head_within_boundary(self._correct_pos, current_pos, self._pad_x, self._pad_y): if not self._alarm.is_playing(): self._alarm.play() else: if self._alarm.is_playing(): self._alarm.stop() else: self._correct_pos = current_pos imgtk = draw_boxes(display_image, self._correct_pos, current_pos, self._pad_x, self._pad_y) self.panel.imgtk = imgtk self.panel.config(image=imgtk) # call the same function after 30 milliseconds self.root.after(50, self._video_loop)
def infer_image(self, num_people, return_overlay=False, return_input_img=False): input_image, display_image, output_scale = p.read_cap( self.video, scale_factor=self.scale_factor, output_stride=self.output_stride) inputs = {self.input_names[0]: input_image} self.client.run(inputs) heatmaps_result = self.client.tensor('heatmap').map() offsets_result = self.client.tensor('offset_2').map() displacement_fwd_result = self.client.tensor( 'displacement_fwd_2').map() displacement_bwd_result = self.client.tensor( 'displacement_bwd_2').map() pose_scores, keypoint_scores, keypoint_coords = p.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=self.output_stride, max_pose_detections=num_people, min_pose_score=0.20) keypoint_coords *= output_scale # for pose in keypoint_coords: # pose[:, 1] += (display_image.shape[1] // 2) - (display_image.shape[0] // 2) if not return_overlay: return keypoint_coords[:num_people] # TODO this isn't particularly fast, use GL for drawing and display someday... overlay_image = p.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.20, min_part_score=0.1) if return_input_img: return cv2.cvtColor( overlay_image, cv2.COLOR_BGR2RGB), keypoint_coords[:num_people], input_image return cv2.cvtColor(overlay_image, cv2.COLOR_BGR2RGB), keypoint_coords[:num_people]
def main(): cam_width = 640 cam_height = 480 scale_factor = 0.7125 with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(101, sess) output_stride = model_cfg['output_stride'] cap = cv2.VideoCapture(0) cap.set(3, cam_width) cap.set(4, cam_height) irr_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor, output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image} ) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=1, min_pose_score=0.15) keypoint_coords *= output_scale overlay_image = posenet.draw_skel_and_kp( display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) cv2.putText(overlay_image, str(irr_count), (0, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 2) cv2.imshow('posenet', overlay_image) # Regularized data before writing to data file: keypoint_coords[:, :, 0] = keypoint_coords[:, :, 0] / 480 keypoint_coords[:, :, 1] = keypoint_coords[:, :, 1] / 640 # Flatten x,y coordinate to side by side data: [x1, y1, x2, y2 ......, x17, y17] flat_array = keypoint_coords.flatten() # Insert the label for training data, stand:0, sit:1, lie:2 to index 0 new_data = np.insert(flat_array, 0, 0) with open("../data/lie.csv", 'w') as data_file data_file.writerow(new_data)
def main(): with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(50, sess) output_stride = model_cfg['output_stride'] cap = cv2.VideoCapture('./640.mp4') cap.set(3, args.cam_width) cap.set(4, args.cam_height) start = time.time() frame_count = 0 avg = 0 people = 0 while cap.isOpened(): input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=20, min_pose_score=0.15) keypoint_coords *= output_scale people += len([i for i in pose_scores if i > 0.15]) frame_count += 1 print("Average people so far: " + str(people / frame_count)) if (frame_count % 150 == 0): print('Average FPS: ', frame_count / (time.time() - start)) print("Sending GET request with avg people.") r = requests.get( url= 'https://api.thingspeak.com/update?api_key=M8ND4LO11PWFJU0Y&field1=' + str(people / frame_count)) if cv2.waitKey(1) & 0xFF == ord('q'): break
def main(): model = posenet.load_model(args.model) model = model.cuda() output_stride = model.output_stride cap = cv2.VideoCapture( "rtsp://*****:*****@192.168.15.220:554/Streaming/channels/402") cap.set(3, args.cam_width) cap.set(4, args.cam_height) start = time.time() frame_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) with torch.no_grad(): input_image = torch.Tensor(input_image).cuda() heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = model( input_image) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multiple_poses( heatmaps_result.squeeze(0), offsets_result.squeeze(0), displacement_fwd_result.squeeze(0), displacement_bwd_result.squeeze(0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale # TODO this isn't particularly fast, use GL for drawing and display someday... overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) cv2.imshow('posenet', overlay_image) frame_count += 1 if cv2.waitKey(1) & 0xFF == ord('q'): break print('Average FPS: ', frame_count / (time.time() - start))
def main(): with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg["output_stride"] if args.file is not None: cap = cv2.VideoCapture(args.file) else: cap = cv2.VideoCapture(args.cam_id) cap.set(3, args.cam_width) cap.set(4, args.cam_height) start = time.time() frame_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) overlay_image, parts_dict = image_utils.process_frame( sess, model_outputs, output_stride, input_image, display_image, output_scale, config.MAX_POSE_DETECTIONS, config.MIN_POSE_SCORE, config.MIN_PART_SCORE, ) image_categorization_dict = image_utils.get_image_categorization_dict( parts_dict) controller.trigger_controls(image_categorization_dict) overlay_image = cv2.flip(overlay_image, 1) cv2.imshow("posenet", overlay_image) frame_count += 1 if cv2.waitKey(1) & 0xFF == ord("q"): break print("Average FPS: ", frame_count / (time.time() - start))
def write_camera(self): cap = cv2.VideoCapture(0) cap.set(3, 960) cap.set(4, 720) while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=self.scale_factor, output_stride=self.output_stride) #print("Pose_ID:",sim.pose_dance(input_image,output_scale)) sim.pose_dance(input_image, output_scale) # TODO this isn't particularly fast, use GL for drawing and display someday... overlay_image = posenet.draw_skel_and_kp(display_image, self.pose_scores, self.keypoint_scores, self.keypoint_coords, min_pose_score=0.15, min_part_score=0.1) cv2.imshow('posenet', overlay_image) #frame_count += 1 if cv2.waitKey(1) & 0xFF == ord('q'): break
def wesbin(ws): while True: with tf.Session() as sess: try: #decode to image img_str = ws.receive() decimg = base64.b64decode(img_str.split(',')[1], validate=True) decimg = Image.open(BytesIO(decimg)) decimg = np.array(decimg, dtype=np.uint8) decimg = cv2.cvtColor(decimg, cv2.COLOR_BGR2RGB) #############your process############### cap = cv2.Canny(decimg, 100, 200) #out_img = decimg cap.set(3, args.cam_width) cap.set(4, args.cam_height) model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] start = time.time() frame_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale # TODO this isn't particularly fast, use GL for drawing and display someday... overlay_image = posenet.draw_skel_and_kp( display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) cv2.imshow('posenet', overlay_image) frame_count += 1 if cv2.waitKey(1) & 0xFF == ord('q'): break #############your process############### #encode to string encimg = cv2.imencode(".jpg", cap, [int(cv2.IMWRITE_JPEG_QUALITY), 80]) img_str = encimg.tostring() img_str = "data:image/jpeg;base64," + base64.b64encode( img_str).decode('utf-8') ws.send(img_str) except: pass
def main(): with tf.Session() as sess: # initialization model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] # Video # source #if args.file is not None: # cap = cv2.VideoCapture(args.file) #else: cap = cv2.VideoCapture(args.cam_id) cap.set(3, args.cam_width) cap.set(4, args.cam_height) start = time.time() frame_count = 0 # create pose slice slice = posenet.pose_slice() while True: # always do step 1 # step 1 input_image, display_image, output_scale = posenet.read_cap(cap, scale_factor=args.scale_factor, output_stride=output_stride) overlay_image = display_image # when main if is not run # main if if frame_count % args.capture_rate == 0: # run posenet # dispaly image is part of step 1 of running posenet pose_scores, kp_scores,kp_coords = run_posenet_from_sess(sess,model_outputs, input_image, scale_factor=args.scale_factor, output_stride=output_stride, num_pose_detections = 3, min_score_in_multi_decode = args.min_pose_score,output_scale=output_scale) # push into stack slice.push_pose(kp_coords, kp_scores, pose_scores) # stack.push(coords) if args.score_angles ==1: pass # draw angles if args.draw_angles ==1: overlay_image = posenet.draw_angles(display_image,pose_scores,kp_coords,limit = args.angle_limit) # overlay_image if args.draw == 1: overlay_image = posenet.draw_skel_and_kp( display_image, pose_scores, kp_scores, kp_coords, min_pose_score=0.15, min_part_score=0.1) cv2.imshow('posenet', overlay_image) frame_count += 1 accel = slice.avg_accel() vel = slice.avg_velocity() score = slice.score_function(func_input = accel/50, noise_input = vel, noise = 0.6) score = 0 if slice.is_chillin() else score print('score: ', score) if cv2.waitKey(1) & 0xFF == ord('q'): break print('Average FPS: ', frame_count / (time.time() - start))
def main(): with tf.Session() as sess: with open(args.match_model) as f: reader = csv.reader(f) motion_model = [row for row in reader] for i in range(len(motion_model)): motion_model[i][1:] = list( map(lambda x: float(x), motion_model[i][1:])) model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] if args.file is not None: cap = cv2.VideoCapture(args.file) else: cap = cv2.VideoCapture(args.cam_id) cap.set(3, args.cam_width) cap.set(4, args.cam_height) start = time.time() frame_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale if pose_scores[0] > 0.5: # clip pose_coords_x = keypoint_coords[0, :, 0] - min( keypoint_coords[0, :, 0]) pose_coords_y = keypoint_coords[0, :, 1] - min( keypoint_coords[0, :, 1]) # normalize x_l2_norm = np.linalg.norm(keypoint_coords[0, :, 0], ord=2) pose_coords_x = (pose_coords_x / x_l2_norm).tolist() y_l2_norm = np.linalg.norm(keypoint_coords[0, :, 1], ord=2) pose_coords_y = (pose_coords_y / y_l2_norm).tolist() distance_min = 1000000 min_num = -1 for teach_num in range(len(motion_model)): distance = weightedDistanceMatching( pose_coords_x, pose_coords_y, keypoint_scores[0, :], pose_scores[0], motion_model[teach_num][1:35]) # distance = cos_sim(pose_coords_x + pose_coords_y, motion_model[teach_num][1:35]) if distance < distance_min: distance_min = distance min_num = teach_num print(distance) cv2.putText(display_image, motion_model[min_num][0], (int(keypoint_coords[0, :, 0][0]), int(keypoint_coords[0, :, 0][1])), cv2.FONT_HERSHEY_SIMPLEX, 2.0, (0, 0, 255), thickness=2) print(motion_model[min_num][0]) overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) cv2.imshow('posenet', overlay_image) # TODO this isn't particularly fast, use GL for drawing and display someday... frame_count += 1 if cv2.waitKey(1) & 0xFF == ord('q'): break print('Average FPS: ', frame_count / (time.time() - start))
def run(self, myLabel): global running global real_start global R_shoulder global R_pelvis global R_knee global R_elbow global L_shoulder global L_pelvis global L_knee global L_elbow #모델 열고 시작 time_count = 0 standing_ratio = 0 with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(args.model, sess) #model_outputs는 텐서 객체들의 리스트 output_stride = model_cfg['output_stride'] if args.file is not None: cap = cv2.VideoCapture(args.file) else: cap = cv2.VideoCapture(0) # peaple_count=1 #한명만 실행 지금 코드가 그대로 되어있음 #이전프레임 저장 기능 복구-----------------------------준영 bf_keyscores=np.zeros((peaple_count,17),float) #이전프레임 저장 기능은 복구 bf_keycoords=np.zeros((peaple_count,17,2),float) #----------------------------------------------------준영 min_pose_score=0.15 #자세 인식 최소값 min_part_score=0.1 #관절 포인트 인식 최소값 angle_save={} L_knee_flag = 3 R_knee_flag = 3 L_hip_flag = 3 R_hip_flag = 3 L_knee_errocounter = 0 #에러표시 유지값 R_knee_errocounter = 0 #에러표시 유지값 L_hip_errocounter = 0 #에러표시 유지값 R_hip_errocounter = 0 #에러표시 유지값 wrongtexts={ (0,0):"", (0,1):"", (1,0):"", (1,1):"" } errocounter={ (0,0):0, (0,1):0, (1,0):0, (1,1):0 } #-----------------------------------------------------연훈 x_arr = np.zeros((17)) y_arr = np.zeros((17)) ratio_arr = [] nose_arr = [] grad_check = [] #----------------------------------------------------은빈 ratio_arr = [] start = time.time() frame_count = 0 while self.running: start_time = time.time() ret, img = cap.read() if ret: try: input_image, display_image, output_scale = posenet.read_cap(cap, scale_factor=args.scale_factor, output_stride=output_stride) #영상입력 except: break #영상을 못받으면 탈출 heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image} ) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=peaple_count, min_pose_score=min_pose_score) keypoint_coords *= output_scale out_img = display_image h,w,c = out_img.shape cv_keypoints = [] cv_keypoints_else = [] #점수보다 낮은 이미지 처리(연훈이형 부분에 없었음) adjacent_keypoints = [] adjacent_keypoints_else = [] #이전프레임 저장 기능 복구-----------------------------준영 errorlist=[] for ii, score in enumerate(pose_scores): #이전 프레임 저장 부분 복구 for jj,(ks, kc) in enumerate(zip(keypoint_scores[ii, :], keypoint_coords[ii, :, :])): #새로인식한 신체 부분 값이 min_part_score보다 높다 or 이전 값보다 높거나 or -0.2하면 음수가 되어버린다 if ks > min_part_score or bf_keyscores[ii][jj]<ks or bf_keyscores[ii][jj]-0.2<0: bf_keyscores[ii][jj]=ks bf_keycoords[ii][jj]=kc else : #기존 값을 사용한다면 최대 5프레임이라는 유통기한을 사용해야할듯 bf_keyscores[ii][jj]-=0.2 errorlist.append(jj) #----------------------------------------------------준영 for ii, score in enumerate(pose_scores): if score < min_part_score: continue results = [] results_else = []#신뢰도 낮은 값 처리를 위한 나머지 리스트 k_s= bf_keyscores[ii, :] k_c= bf_keycoords[ii, :, :] for left, right in posenet.CONNECTED_PART_INDICES: #선찾기 if k_s[left] < min_part_score or k_s[right] < min_part_score:#값보다 낮으면 낮은 쪽에 넣고 results_else.append(np.array([k_c[left][::-1], k_c[right][::-1]]).astype(np.int32),) else :#값보다 높으면 높은 쪽에 넣고 results.append(np.array([k_c[left][::-1], k_c[right][::-1]]).astype(np.int32),) adjacent_keypoints.extend(results)#값보다 높은 것들의 묶음은 높은 쪽에 넣고 adjacent_keypoints_else.extend(results_else)#값보다 낮은 것들의 묶음은 낮은 쪽에 넣고 for jj, (ks, kc) in enumerate(zip(k_s, k_c)):#점찾기 if ks < min_part_score:#값보다 낮으면 낮은 쪽에 넣고 cv_keypoints_else.append(cv2.KeyPoint(kc[1], kc[0], 10. * ks)) else:#값보다 높으면 높은 쪽에 넣고 cv_keypoints.append(cv2.KeyPoint(kc[1], kc[0], 10. * ks)) out_img = cv2.putText(out_img, parts[jj], (int(kc[1]), int(kc[0])), cv2.FONT_HERSHEY_SIMPLEX, 0.2, (0,0,0), 1, cv2.LINE_AA)#그 점 좌표에 관절명을 적음 if parts[jj] in angle_dict: temp_angle = angle_cal(k_c[angle_dict[parts[jj]][0]],k_c[angle_dict[parts[jj]][1]],k_c[angle_dict[parts[jj]][2]]) out_img = cv2.putText(out_img, str(int(temp_angle)), (int(kc[1]), int(kc[0])), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,255,255), 1, cv2.LINE_AA)#그 점 좌표에 관절명을 적음 for i,v in angle_dict.items(): angle_save[i]=int(angle_cal(k_c[v[0]],k_c[v[1]],k_c[v[2]])) #------------------------------신은빈 stading pose 검출------------------------------ angle_save['R shoulder'] = int(angle_cal(k_c[angle_dict['R shoulder'][0]],k_c[angle_dict['R shoulder'][1]],k_c[angle_dict['R shoulder'][2]])) angle_save['R elbow'] = int(angle_cal(k_c[angle_dict['R elbow'][0]],k_c[angle_dict['R elbow'][1]],k_c[angle_dict['R elbow'][2]])) angle_save['R pelvis'] = int(angle_cal(k_c[angle_dict['R pelvis'][0]],k_c[angle_dict['R pelvis'][1]],k_c[angle_dict['R pelvis'][2]])) angle_save['R knee'] = int(angle_cal(k_c[angle_dict['R knee'][0]],k_c[angle_dict['R knee'][1]],k_c[angle_dict['R knee'][2]])) angle_save['L shoulder'] = int(angle_cal(k_c[angle_dict['L shoulder'][0]],k_c[angle_dict['L shoulder'][1]],k_c[angle_dict['L shoulder'][2]])) angle_save['L elbow'] = int(angle_cal(k_c[angle_dict['L elbow'][0]],k_c[angle_dict['L elbow'][1]],k_c[angle_dict['L elbow'][2]])) angle_save['L pelvis'] = int(angle_cal(k_c[angle_dict['L pelvis'][0]],k_c[angle_dict['L pelvis'][1]],k_c[angle_dict['L pelvis'][2]])) angle_save['L knee'] = int(angle_cal(k_c[angle_dict['L knee'][0]],k_c[angle_dict['L knee'][1]],k_c[angle_dict['L knee'][2]])) #----------------------------------------------------연훈이형 코드(:코의 속도가 음수일때, 현재 무릎 각도를 측정하여/높이를 낮추고 있다면, 아직 부족하다를 보여줌) # 발목과 코의 위치 비율 구하기 left_ankle_y = k_c[15][0] nose_y = k_c[0][0] ratio = (left_ankle_y / nose_y) ratio_arr.append(ratio) nose_arr.append(k_c[0][0]) #================================================================ #정상 포즈 판별 end_time = time.time() if real_start == False: if 'R shoulder' in angle_save: if angle_save['R shoulder'] >=0 and angle_save['R shoulder']<=35: R_shoulder = True else: time_count = 0 print(str(angle_save['R shoulder']),'R shoulder False') R_shoulder = False if 'L shoulder' in angle_save: if angle_save['L shoulder'] >=0 and angle_save['L shoulder']<=35: L_shoulder = True else: time_count = 0 print(str(angle_save['L shoulder']),'L shoulder False') L_shoulder = False if 'R elbow' in angle_save: if angle_save['R elbow'] >=145 and angle_save['R elbow']<=205: R_elbow = True else: time_count = 0 print(str(angle_save['R elbow']),'R elbow F') R_elbow = False if 'L elbow' in angle_save: if angle_save['L elbow'] >=145 and angle_save['L elbow']<=205: L_elbow = True else: time_count = 0 print(str(angle_save['L elbow']),'L elbow F') L_elbow = False if 'R pelvis' in angle_save: if angle_save['R pelvis'] >=65 and angle_save['R pelvis']<=140: R_pelvis = True else: time_count = 0 print(str(angle_save['R pelvis']),'R pelvis F') R_pelvis = False if 'L pelvis' in angle_save: if angle_save['L pelvis'] >=65 and angle_save['L pelvis']<=140: L_pelvis = True else: time_count = 0 print(str(angle_save['L pelvis']),'L pelvis F') L_pelvis = False if 'R knee' in angle_save: if angle_save['R knee'] >=140 and angle_save['R knee']<=200: R_knee = True else: time_count = 0 print(str(angle_save['R knee']),'R knee F') R_knee = False if 'L knee' in angle_save: if angle_save['L knee'] >=140 and angle_save['L knee']<=200: L_knee = True else: time_count = 0 print(str(angle_save['L knee']),'L knee F') L_knee = False if R_shoulder & R_elbow & R_pelvis & R_knee & L_shoulder & L_elbow & L_pelvis & L_knee: time_count += end_time - start_time #print(int(time_count)) out_img = cv2.putText(out_img, str(int(time_count)), (50, 80), cv2.FONT_HERSHEY_DUPLEX, 1.5, (0,0,0), 1, cv2.LINE_AA) if time_count > 3: real_start = True standing_ratio = (left_ankle_y / nose_y) else: time_count = 0 out_img = cv2.putText(out_img, 'Please Re-pose', (50, 80), cv2.FONT_HERSHEY_DUPLEX, 1.5, (0,0,0), 1, cv2.LINE_AA) #----------------------------------------------------좌표 위치 그대로 그려주는 코드 out_img = cv2.drawKeypoints(out_img, cv_keypoints, outImage=np.array([]), color=(255, 255, 0),flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS) out_img = cv2.drawKeypoints(out_img, cv_keypoints_else, outImage=np.array([]), color=(0, 0, 255),flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS) out_img = cv2.polylines(out_img, adjacent_keypoints, isClosed=False, color=(255, 255, 0)) out_img = cv2.polylines(out_img, adjacent_keypoints_else, isClosed=False, color=(0, 0, 255)) #================================================================ if real_start: grad = (mean(nose_arr[-10:-5]) - mean(nose_arr[-5:-1]))/10 grad_check.append(grad) # 자세 판별 squat_knee_angle = 90 # left_knee_angle_arr.append(left_knee_angle) left_knee_hip_gap = abs(k_c[11][0] - k_c[13][0]) right_knee_hip_gap = abs(k_c[12][0] - k_c[14][0]) global test global color squat_down(ratio, ratio_arr, grad, squat_knee_angle, angle_save["L knee"], angle_save["R knee"], left_knee_hip_gap, right_knee_hip_gap, standing_ratio) out_img=cv2.putText(out_img, test, (50,100), cv2.FONT_HERSHEY_DUPLEX, 2, color=color, thickness=2) #<운동체커 0:내려가는중 1:올라가는중 2:최저 정상범위 3:최고 정상범위 4:운동 오류> if angle_save: # wrongtexts={} 유지를 위해 더 상위에 선언 L_knee_flag,errocounter[(1,1)],wrongtexts[(1,1)]=angle_flag(angle_save["L knee"],L_knee_mins,L_knee_maxs,L_knee_flag,errocounter[(1,1)],"L knee",wrongtexts[(1,1)]) # L_knee_flag,L_knee_errocounter,out_img=angle_text(0,0,L_knee_flag,L_knee_errocounter,out_img) #------------------------------------------오른쪽 무릎 코드 R_knee_flag,errocounter[(0,1)],wrongtexts[(0,1)]=angle_flag(angle_save["R knee"],R_knee_mins,R_knee_maxs,R_knee_flag,errocounter[(0,1)],"R knee",wrongtexts[(0,1)]) # R_knee_flag,R_knee_errocounter,out_img=angle_text(1,0,R_knee_flag,R_knee_errocounter,out_img) #------------------------------------------왼쪽 옆구리 코드 L_hip_flag,errocounter[(1,0)],wrongtexts[(1,0)]=angle_flag(angle_save["L hip"],L_hip_mins,L_hip_maxs,L_hip_flag,errocounter[(1,0)],"L hip",wrongtexts[(1,0)]) # L_hip_flag,L_hip_errocounter,out_img=angle_text(0,1,L_hip_flag,L_hip_errocounter,out_img) #------------------------------------------오른쪽 옆구리 코드 R_hip_flag,errocounter[(0,0)],wrongtexts[(0,0)]=angle_flag(angle_save["R hip"],R_hip_mins,R_hip_maxs,R_hip_flag,errocounter[(0,0)],'R hip',wrongtexts[(0,0)]) # R_hip_flag,R_hip_errocounter,out_img=angle_text(1,1,R_hip_flag,R_hip_errocounter,out_img) errocounter,wrongtexts,out_img=angle_text(errocounter,wrongtexts,out_img) #print("LN:{},{:.1f}\tRN:{},{:.1f}\tLH:{},{:.1f}\tRT:{},{:.1f}".format(L_knee_flag,angle_save['L knee'],R_knee_flag,angle_save['R knee'],L_hip_flag,angle_save['L hip'],R_hip_flag,angle_save['R hip'])) out_img = cv2.cvtColor(out_img, cv2.COLOR_BGR2RGB) #time.sleep(0.05) qImg = QtGui.QImage(out_img.data, w, h, w*c, QtGui.QImage.Format_RGB888) pixmap = QtGui.QPixmap.fromImage(qImg) myLabel.setPixmap(pixmap) cap.release() print("Thread end.")
def main(): with tf.Session() as sess: # Load the models model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] if args.file is not None: # Frame source, speicifed file or the specified(or default) live cam cap = cv2.VideoCapture(args.file) else: cap = cv2.VideoCapture(args.cam_id) cap.set(3, args.cam_width) cap.set(4, args.cam_height) previous_pose = '' # '' denotes it is empty, really fast checking! count = 0 # Stores the count of repetitions # A flag denoting change in state. 0 -> previous state is continuing, 1 -> state has changed flag = -1 # Novel string stores a pair of bits for each of the 12 joints denoting whether the joint is moving up or down # when plotted in a graph against time, 1 denotes upward and 0 denotes downward curving of the graph. It is initialised # as '22' so that current_state wont ever be equal to the string we generate unless there is no movement out of tolerance current_state = [2, 2] while True: # Get a frame, and get the model's prediction input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.4) keypoint_coords *= output_scale # Normalising the output against the resolution if ( isinstance(previous_pose, str) ): # if previous_pose was not inialised, assign the current keypoints to it previous_pose = keypoint_coords text, previous_pose, current_state, flag = countRepetition( previous_pose, keypoint_coords, current_state, flag) if (flag == 1): count += 1 flag = -1 image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.4, min_part_score=0.1) # OpenCV does not recognise the use of \n delimeter y0, dy = 20, 20 for i, line in enumerate(text.split('\n')): y = y0 + i * dy image = cv2.putText(image, line, (10, y), cv2.FONT_HERSHEY_SIMPLEX, .5, (0, 0, 255), 3) cal_burn = round(count * 0.2625, 2) text = (f"count:{count} cal_burn:{cal_burn}") image = cv2.putText(image, text, (950, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 4) cv2.imshow('RepCounter', image) ch = cv2.waitKey(1) if (ch == ord('q') or ch == ord('Q')): break # Exit the loop on press of q or Q elif (ch == ord('r') or ch == ord('R')): count = 0 cap.release() cv2.destroyAllWindows()
thickness = 2 model = posenet.load_model(101) if train_on_gpu: model = model.cuda() output_stride = model.output_stride cap = cv2.VideoCapture(0) start = time.time() frame_count = 0 rows = [200, 200] while True: arr = [] input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=0.7125, output_stride=output_stride) with torch.no_grad(): if train_on_gpu: input_image = torch.Tensor(input_image).cuda() else: input_image = torch.Tensor(input_image) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = model(input_image) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multiple_poses( heatmaps_result.squeeze(0), offsets_result.squeeze(0), displacement_fwd_result.squeeze(0), displacement_bwd_result.squeeze(0), output_stride=output_stride,
def main(): with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] if args.file is not None: cap = cv2.VideoCapture(args.file) else: cap = cv2.VideoCapture(args.cam_id) cap.set(3, args.cam_width) cap.set(4, args.cam_height) start = time.time() frame_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale # TODO this isn't particularly fast, use GL for drawing and display someday... overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) s1 = [] d = 0 #cv2.putText(overlay_image,str(frame_count / (time.time() - start)),(320,320),cv2.FONT_HERSHEY_SIMPLEX,1,(255, 0, 0)) for pi in range(len(pose_scores)): if pose_scores[pi] == 0.: break #cv2.putText(overlay_image,str(pi),(int(keypoint_coords[pi,0,0]),int(keypoint_coords[pi,0,1])),cv2.FONT_HERSHEY_SIMPLEX,1,(255, 0, 0),) # print(keypoint_coords[pi, 5, :]) # print(keypoint_coords[pi, 9, :]) print(keypoint_coords[pi, 0, 0], keypoint_coords[pi, 0, 1]) # 3 ft distance x1 = keypoint_coords[pi, 5, 0] y1 = keypoint_coords[pi, 5, 1] x2 = keypoint_coords[pi, 9, 0] y2 = keypoint_coords[pi, 9, 1] d = dist(x1, y1, x2, y2) s1.append(d) if not len(s1) == 0: d = max(s1) #if not d==0: #print(d) arr = numpy.array(pose_scores) arr = arr[arr != 0] arr = arr.tolist() all_combinations = [] combinations_object = itertools.combinations(range(len(arr)), 2) all_combinations = list(combinations_object) # if len(all_combinations) > 0: # print(all_combinations) # # if len(all_combinations)>0: # print(all_combinations[0][0] + all_combinations[0][1]) if len(all_combinations) > 0: for i1 in range(len(all_combinations)): c1 = all_combinations[i1][0] c2 = all_combinations[i1][1] if dist(keypoint_coords[c1, 0, 0], keypoint_coords[c1, 0, 1], keypoint_coords[c2, 0, 0], keypoint_coords[c2, 0, 1]) > d: cv2.putText(overlay_image, "violated", (320, 320), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0)) cv2.imshow('posenet', overlay_image) frame_count += 1 if cv2.waitKey(1) & 0xFF == ord('q'): break
def main(): with tf.Session() as sess: model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] if args.file is not None: cap = cv2.VideoCapture(args.file) else: cap = cv2.VideoCapture(args.cam_id) cap.set(3, args.cam_width) cap.set(4, args.cam_height) hasFrame, frame = cap.read() # print(frame.shape) start = time.time() frame_count = 0 prvs = cv2.resize(frame, (224, 224)) prvs = cv2.cvtColor(prvs, cv2.COLOR_BGR2GRAY) hsv = np.zeros((224, 224, 3), dtype=np.uint8) hsv[..., 1] = 255 #intensity c = 0 while True: c += 1 flag = 0 t = time.time() hasFrame, frame = cap.read() if not hasFrame: break next = cv2.resize(frame, (224, 224)) # print(next.shape) next = cv2.cvtColor(next, cv2.COLOR_BGR2GRAY) prvs = cv2.medianBlur(prvs, 5) next = cv2.medianBlur(next, 5) # print(prvs.shape,next.shape) flow = cv2.calcOpticalFlowFarneback(prvs, next, None, 0.5, 3, 7, 4, 7, 5, 0) mag, ang = cv2.cartToPolar(flow[..., 0], flow[..., 1]) mag = (mag > 1.4) * mag hsv[..., 0] = ang * 180 / np.pi / 2 #hue, colour hsv[..., 2] = cv2.normalize(mag, None, 0, 255, cv2.NORM_MINMAX) #brightness up_mask = make_mask(hsv[..., 0], 125, 150) #purple down_mask = make_mask(hsv[..., 0], 35, 75) #green left_mask = make_mask(hsv[..., 0], 165, 179) | make_mask( hsv[..., 0], 0, 30) #red right_mask = make_mask(hsv[..., 0], 75, 105) #blue #label machine # up_mask = make_mask(hsv[...,0],125,150) #purple # down_mask = make_mask(hsv[...,0],35,75) #green # left_mask = make_mask(hsv[...,0],165,179) | make_mask(hsv[...,0],1,20)#red # right_mask = make_mask(hsv[...,0],80,100) #blue hsv_up = apply_mask(hsv, up_mask) hsv_down = apply_mask(hsv, down_mask) hsv_left = apply_mask(hsv, left_mask) hsv_right = apply_mask(hsv, right_mask) input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=1, min_pose_score=0.15) keypoint_coords *= output_scale #for i in range(len(pose_scores)): for pts in keypoint_coords: dist1 = [0] dist2 = [0] if int(pts[9, 0]) > 0 and int(pts[9, 0]) < 140 and int( pts[9, 1]) > 0 and int(pts[9, 1]) < 140: cv2.circle(display_image, (int(pts[9, 1]), int(pts[9, 0]) - 20), 20, (0, 255, 0), 2) cv2.circle(display_image, (int(pts[9, 1]), int(pts[9, 0]) + 30), 20, (0, 255, 0), 2) if int(pts[10, 0]) > 0 and int(pts[10, 0]) < 140 and int( pts[10, 1]) > 0 and int(pts[10, 1]) < 140: cv2.circle(display_image, (int(pts[10, 1]), int(pts[10, 0]) - 20), 20, (0, 255, 0), 2) cv2.circle(display_image, (int(pts[10, 1]), int(pts[10, 0]) + 30), 20, (0, 255, 0), 2) valid_pts = 0 for i in range(len(mag)): for j in range(len(mag[0])): if mag[j, i] > 0: if (isInsideC(int(pts[9, 1]), int(pts[9, 0]) - 20, 20, j, i) or isInsideC(int(pts[9, 1]), int(pts[9, 0]) + 30, 20, j, i) ) and (int(pts[9, 0]) > 0 and int(pts[9, 0]) < 140 and int(pts[9, 1]) > 0 and int(pts[9, 1]) < 140): valid_pts += 1 if (isInsideC(int(pts[10, 1]), int(pts[10, 0]) - 20, 20, j, i) or isInsideC( int(pts[10, 1]), int(pts[10, 0]) + 30, 20, j, i)) and (int(pts[10, 0]) > 0 and int(pts[10, 0]) < 140 and int(pts[10, 1]) > 0 and int(pts[10, 1]) < 140): valid_pts += 1 # dist1.append(dist_from_line(j,i,pts[7,:],pts[9,:])) # left hand # dist2.append(dist_from_line(j,i,pts[8,:],pts[10,:]))# right hand # dist1.append(dist_from_pt(j,i,pts[9,:])) # left wrist # dist2.append(dist_from_pt(j,i,pts[10,:])) # right wrist # print(pts[9,:]) # print(pts[10,:]) # print(valid_pts) # print(np.mean(dist1),np.mean(dist2)) thresh = 140 up_thresh = 34 down_thresh = 14 left_thresh = 24 right_thresh = 28 # if (np.mean(dist1) < thresh or np.mean(dist2)<thresh) and (np.mean(dist1) >0 and np.mean(dist2)>0): # if True: if valid_pts > 100: #original # print('please print') # print(np.mean(hsv_right[...,0])) if np.mean(hsv_up[..., 0]) > up_thresh and np.mean(mag) > 0.07: # print(np.mean(hsv_up[...,0])) print('UP', c) flag = 1 if np.mean(hsv_down[..., 0]) > down_thresh and np.mean(mag) > 0.07: # print(np.mean(hsv_down[...,0])) print('DOWN', c) flag = 1 if np.mean(hsv_left[..., 0]) > left_thresh and np.mean(mag) > 0.07: # print(np.mean(hsv_left[...,0])) print('LEFT', c) flag = 1 if np.mean( hsv_right[..., 0]) > right_thresh and np.mean(mag) > 0.07: # print(np.mean(hsv_right[...,0])) print('RIGHT', c) flag = 1 #modified # if np.mean(hsv_up[...,0])>38 and np.mean(mag)>0.08: # print('UP',np.mean(hsv_up[...,0])) # flag = 1 # if np.mean(hsv_down[...,0])>16.5 and np.mean(mag)>0.08: # print('DOWN',np.mean(hsv_down[...,0])) # flag = 1 # if np.mean(hsv_left[...,0])>24 and np.mean(mag)>0.08: # print('LEFT',c) # flag = 1 # if np.mean(hsv_right[...,0])>28 and np.mean(mag)>0.08: # print('RIGHT',c) # flag = 1 # TODO this isn't particularly fast, use GL for drawing and display someday... overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) overlay_image = cv2.resize(overlay_image, (224, 224)) bgru = cv2.cvtColor(hsv_up, cv2.COLOR_HSV2BGR) bgrd = cv2.cvtColor(hsv_down, cv2.COLOR_HSV2BGR) bgrl = cv2.cvtColor(hsv_left, cv2.COLOR_HSV2BGR) bgrr = cv2.cvtColor(hsv_right, cv2.COLOR_HSV2BGR) # bgr = cv2.medianBlur(bgr,5) cv2.imshow('flow_up', bgru) cv2.imshow('flow_down', bgrd) cv2.imshow('flow_left', bgrl) cv2.imshow('flow_right', bgrr) cv2.imshow('posenet', overlay_image) prvs = next frame_count += 1 if cv2.waitKey(1) & 0xFF == ord('q'): break print('Average FPS: ', frame_count / (time.time() - start))
def main(): with tf.compat.v1.Session() as sess: model_cfg, model_outputs = posenet.load_model(args.model, sess) output_stride = model_cfg['output_stride'] # if args.file is not None: # cap = cv2.VideoCapture(args.file) # else: cap = cv2.VideoCapture(0) # cap = cv2.VideoCapture(args.cam_id) # cap.set(3, args.cam_width) # cap.set(4, args.cam_height) start = time.time() frame_count = 0 while True: input_image, display_image, output_scale = posenet.read_cap( cap, scale_factor=args.scale_factor, output_stride=output_stride) heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run( model_outputs, feed_dict={'image:0': input_image}) pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses( heatmaps_result.squeeze(axis=0), offsets_result.squeeze(axis=0), displacement_fwd_result.squeeze(axis=0), displacement_bwd_result.squeeze(axis=0), output_stride=output_stride, max_pose_detections=10, min_pose_score=0.15) keypoint_coords *= output_scale # TODO this isn't particularly fast, use GL for drawing and display someday... overlay_image = posenet.draw_skel_and_kp(display_image, pose_scores, keypoint_scores, keypoint_coords, min_pose_score=0.15, min_part_score=0.1) # print(overlay_image.shape,"shape") overlay_image = cv2.resize(overlay_image, (500, 480)) cv2.imshow('posenet', overlay_image) frame_count += 1 if cv2.waitKey(1) & 0xFF == ord('q'): break # print(keypoint_coords[0][5],'sholder') # print(keypoint_coords[0][7],'elbow') # print(keypoint_coords[0][9],'wrist') l_shoulder = keypoint_coords[0][5] r_shoulder = keypoint_coords[0][6] l_elbow = keypoint_coords[0][7] r_elbow = keypoint_coords[0][8] l_wrist = keypoint_coords[0][9] r_wrist = keypoint_coords[0][10] l_hip = keypoint_coords[0][11] r_hip = keypoint_coords[0][12] l_knee = keypoint_coords[0][13] r_knee = keypoint_coords[0][14] angle_l_elbow, c_l_e = get_anglel(l_wrist, l_elbow, l_shoulder) angle_r_elbow, c_r_e = get_angler(r_wrist, r_elbow, r_shoulder) angle_r_shoulder = (get_angle2(r_elbow, r_shoulder, r_hip)) angle_l_shoulder = (get_angle2(l_elbow, l_shoulder, l_hip)) angle_l_leg = get_angle2(l_knee, l_hip, l_shoulder) angle_r_leg = get_angle2(r_knee, r_hip, r_shoulder) angle_h = get_angleh((l_shoulder + r_shoulder) / 2.0, (l_hip + r_hip) / 2.0) # print(angle_h) if angle_r_leg > 90: angle_r_leg = np.abs(180 - angle_r_leg) if angle_l_leg > 90: angle_l_leg = np.abs(180 - angle_l_leg) # print(angle_r_shoulder,"right",c_r_e) # print(get_angle(l_shoulder,l_elbow,l_wrist)) # print(angle_l_elbow,"left",c_l_e) l_elbow_position = (angle_l_elbow) * 3.141 / 180 l_shoulder_position = (90 - angle_l_shoulder) * 3.141 / 180 l_elbow_joint = 20 l_shoulder_joint = 18 r_elbow_joint = 24 r_shoulder_joint = 22 l_leg_upper_joint = 5 r_leg_upper_joint = 0 # print(angle_l_elbow,"angle",c_l_e) # print(angle_r_leg,"right leggg") if (angle_r_elbow > 180): angle_r_elbow = 180 - angle_r_elbow r_elbow_position = (angle_r_elbow) * 3.141 / 180 r_shoulder_position = (90 - angle_r_shoulder) * 3.141 / 180 l_leg_position = angle_l_leg * 3.141 / 180.0 * -1 r_leg_position = angle_r_leg * 3.141 / 180.0 h_position = angle_h * 3.141 / 180.0 p.setJointMotorControl2(robot, 17, p.POSITION_CONTROL, targetPosition=1.57, force=500) if c_l_e >= 0: p.setJointMotorControl2(robot, 19, p.POSITION_CONTROL, targetPosition=-1.57, force=500) p.setJointMotorControl2(robot, l_elbow_joint, p.POSITION_CONTROL, targetPosition=-1 * l_elbow_position, force=5000) else: p.setJointMotorControl2(robot, 19, p.POSITION_CONTROL, targetPosition=1.57, force=500) p.setJointMotorControl2(robot, l_elbow_joint, p.POSITION_CONTROL, targetPosition=-1 * (np.pi - l_elbow_position), force=5000) p.setJointMotorControl2(robot, l_shoulder_joint, p.POSITION_CONTROL, targetPosition=l_shoulder_position, force=5000) if c_r_e <= 0: p.setJointMotorControl2(robot, 23, p.POSITION_CONTROL, targetPosition=-1.57, force=500) p.setJointMotorControl2(robot, r_elbow_joint, p.POSITION_CONTROL, targetPosition=(np.pi - r_elbow_position), force=500) else: p.setJointMotorControl2(robot, 23, p.POSITION_CONTROL, targetPosition=1.57, force=500) p.setJointMotorControl2(robot, r_elbow_joint, p.POSITION_CONTROL, targetPosition=r_elbow_position, force=500) p.setJointMotorControl2(robot, 21, p.POSITION_CONTROL, targetPosition=1.57, force=500) p.setJointMotorControl2(robot, 11, p.POSITION_CONTROL, targetPosition=h_position, force=500) # p.setJointMotorControl2(robot, 23, p.POSITION_CONTROL, targetPosition =-1.57,force = 500) # p.setJointMotorControl2(robot, r_elbow_joint, p.POSITION_CONTROL, targetPosition =3.141-r_elbow_position,force = 500) p.setJointMotorControl2(robot, r_shoulder_joint, p.POSITION_CONTROL, targetPosition=r_shoulder_position, force=500) p.setJointMotorControl2(robot, r_leg_upper_joint, p.POSITION_CONTROL, targetPosition=r_leg_position, force=500) p.setJointMotorControl2(robot, l_leg_upper_joint, p.POSITION_CONTROL, targetPosition=l_leg_position, force=500) p.stepSimulation()