internal static bool checkPanelRotationGesture(double horizontal_stretch_sensitivity, ref ArmPosition left_arm_position, ref ArmPosition right_arm_position) { // Calling the static method to update the arm position states: return(panelRotationInitialCondition(horizontal_stretch_sensitivity, ref left_arm_position, ref right_arm_position)); }
// This trigger position is when the arms are stretched out horizontally: private static bool triggerPosition_1(double horizontal_stretch_sensitivity, ref ArmPosition left_arm_position, ref ArmPosition right_arm_position) { bool ret_bool = false; // We first check to see if ShoulderCenter is above the two other shoulders: if (GesturesMasterControl.body.Joints[JointType.SpineShoulder].Position.Y > GesturesMasterControl.body.Joints[JointType.ShoulderLeft].Position.Y && GesturesMasterControl.body.Joints[JointType.SpineShoulder].Position.Y > GesturesMasterControl.body.Joints[JointType.ShoulderRight].Position.Y) { double shoulder_left_Y = GesturesMasterControl.body.Joints[JointType.ShoulderLeft].Position.Y; double shoulder_right_Y = GesturesMasterControl.body.Joints[JointType.ShoulderRight].Position.Y; double elbow_left_Y = GesturesMasterControl.body.Joints[JointType.ElbowLeft].Position.Y; double elbow_right_Y = GesturesMasterControl.body.Joints[JointType.ElbowRight].Position.Y; // Then we check to see if the height between shoulder and elbow is not too much: if (Math.Abs(shoulder_left_Y - elbow_left_Y) < horizontal_stretch_sensitivity && Math.Abs(shoulder_right_Y - elbow_right_Y) < horizontal_stretch_sensitivity) { left_arm_position = ArmPosition.ArmStretchedOutHorizontally; right_arm_position = ArmPosition.ArmStretchedOutHorizontally; ret_bool = true; } else { left_arm_position = ArmPosition.Unknown; right_arm_position = ArmPosition.Unknown; } } return(ret_bool); }
public Task(int Id, BodyPosition Body, ArmPosition Arm, DOF dof, InputType Input = InputType.TRIGGER) { this.Id = Id; this.Round = 0; this.BodyPos = Body; this.ArmPos = Arm; this.DegreeOfFreedom = dof; this.Input = Input; this.Circles = new List <Circle>(); this.Stack = new List <Position>(); }
internal static bool panelRotationInitialCondition(double horizontal_stretch_sensitivity, ref ArmPosition left_arm_position, ref ArmPosition right_arm_position) { bool ret_bool; /*ret_bool = Gesture_PanelRotation.triggerPosition_1(horizontal_stretch_sensitivity, ref left_arm_position, * ref right_arm_position);*/ ret_bool = triggerPosition_2(.3, .3, ref left_arm_position, ref right_arm_position); return(ret_bool); }
static byte tableLenght = 255; // Duzina stola (u ciklusima ScanMovementa) // Resetuje ruku na polozaj 0 (Open) static void ArmCalibrate() { LcdConsole.WriteLine("Kalibracija ruke..."); armMotor.ResetTacho(); armMotor.SetPower(-C.armCalibrationPower); Thread.Sleep(C.armCalibrationTime); armMotor.Off(); Thread.Sleep(500); armMotor.ResetTacho(); armPos = ArmPosition.Open; LcdConsole.WriteLine("Kalibracija završena"); }
// Pomera ruku na odredjeni polozaj static void ArmMove(ArmPosition targetPos) { LcdConsole.WriteLine("Pomeranje ruke na položaj {0}", targetPos.ToString()); int armTacho = armMotor.GetTachoCount(); int direction = Math.Sign((uint)targetPos - armTacho); uint steps = (uint)Math.Abs((uint)targetPos - armTacho); sbyte speed = (sbyte)(C.armSpeed * direction); motorWH = armMotor.SpeedProfile(speed, 0, steps, 0, true); motorWH.WaitOne(); armPos = targetPos; if (armPos != ArmPosition.Lifted) { armMotor.Off(); } LcdConsole.WriteLine("Pozicija motora ruke: {0}", armMotor.GetTachoCount().ToString()); }
public Gesture_PanelRotation(ArmPosition left_arm_position, ArmPosition right_arm_position, double horizontal_stretch_sensitivity) { angle_buffer = new double[BUFFER_LENGTH]; buffer_filled_spots = 0; this.horizontal_stretch_sensitivity = horizontal_stretch_sensitivity; left_arm_rotation = new Rotations(); right_arm_rotation = new Rotations(); this.left_arm_position = left_arm_position; this.right_arm_position = right_arm_position; shoulder_to_elbow_left = new Vector3D(); shoulder_to_elbow_right = new Vector3D(); average_angle = 0.0; }
/* 1. Select a person most likely for gesture * 2. Wait until triggering conditions are met * 3. Wait for 0.5 seconds when triggered before starting the reading * 4. Read while checking the boundaries * 5. When a rotation is made, and the hands are still again, send a bluetooth signal * a. at each correct angle reading, measure the time spent at that angle * b. if the time exceeds some threshhold (1 second), send a signal * c. if the person wants to continue rotating, he needs to wait a particular amount of time (based on motor rotation speed) * d. reading to commence again for a new rotation * 6. Wait until another rotation is made */ /* Constructor: */ public GesturesMasterControl() { stopWatch = new Stopwatch(); stopWatch_single_event = new Stopwatch(); stopWatch_trigger = new Stopwatch(); running_gesture = GestureRunningState.Unknown; right_elbow_general_state = JointGeneralState.Unknown; left_elbow_general_state = JointGeneralState.Unknown; right_arm_position = ArmPosition.Unknown; left_arm_position = ArmPosition.Unknown; lf_elbow_prev_pos = new Joint[PREV_FRAMES_ARRAY_LENGTH]; rt_elbow_prev_pos = new Joint[PREV_FRAMES_ARRAY_LENGTH]; shoulder_to_elbow_left = new Vector3D(); shoulder_to_elbow_right = new Vector3D(); elbow_to_hand_left = new Vector3D(); elbow_to_hand_right = new Vector3D(); }
public MoveCommand(ArmPosition position) { Position = (ArmPosition)position.Clone(); }
public MoveCommand(double x, double y, double z) { Position = new ArmPosition(x, y, z); }
/// <summary> /// This method is designed to trigger a gesture at any angle, not just horizontally stretched. /// </summary> /// <param name="vert_arm_diff_sensitivity"></param> /// <param name="position_sensitivity"></param> /// <param name="left_arm_position"></param> /// <param name="right_arm_position"></param> /// <returns></returns> private static bool triggerPosition_2(double vert_arm_diff_sensitivity, double position_sensitivity, ref ArmPosition left_arm_position, ref ArmPosition right_arm_position) { bool short_arm_limb_steady = false, long_arm_limb_steady = false, right_Z_dist_steady = false, left_Z_dist_steady = false; // We get the Y differences between all arm joints: double left_Y_shoul_elbow_diff = GesturesMasterControl.body.Joints[JointType.ShoulderLeft].Position.Y - GesturesMasterControl.body.Joints[JointType.ElbowLeft].Position.Y; double right_Y_shoul_elbow_diff = GesturesMasterControl.body.Joints[JointType.ShoulderRight].Position.Y - GesturesMasterControl.body.Joints[JointType.ElbowRight].Position.Y; double left_Y_elbow_wrist_diff = GesturesMasterControl.body.Joints[JointType.ElbowLeft].Position.Y - GesturesMasterControl.body.Joints[JointType.WristLeft].Position.Y; double right_Y_elbow_wrist_diff = GesturesMasterControl.body.Joints[JointType.ElbowRight].Position.Y - GesturesMasterControl.body.Joints[JointType.WristRight].Position.Y; double left_Y_shoul_wrist_diff = GesturesMasterControl.body.Joints[JointType.ShoulderLeft].Position.Y - GesturesMasterControl.body.Joints[JointType.WristLeft].Position.Y; double right_Y_shoul_wrist_diff = GesturesMasterControl.body.Joints[JointType.ShoulderRight].Position.Y - GesturesMasterControl.body.Joints[JointType.WristRight].Position.Y; double shoulder_spine_Y = GesturesMasterControl.body.Joints[JointType.SpineShoulder].Position.Y; double left_elbow_Y = GesturesMasterControl.body.Joints[JointType.ElbowLeft].Position.Y; double right_elbow_Y = GesturesMasterControl.body.Joints[JointType.ElbowRight].Position.Y; // First lets check to see if the elbows are not both above or below the spine shoulder: if ((left_elbow_Y - shoulder_spine_Y > 0 && right_elbow_Y - shoulder_spine_Y > 0) || (left_elbow_Y - shoulder_spine_Y < 0 && right_elbow_Y - shoulder_spine_Y < 0)) { // Check if both elbows are not horizontal: if (Math.Abs(left_elbow_Y - shoulder_spine_Y) > trigg_2_horizontal_sensitivity && Math.Abs(right_elbow_Y - shoulder_spine_Y) > trigg_2_horizontal_sensitivity) { return(false); } } left_Y_shoul_elbow_diff = Math.Abs(left_Y_shoul_elbow_diff); right_Y_shoul_elbow_diff = Math.Abs(right_Y_shoul_elbow_diff); left_Y_elbow_wrist_diff = Math.Abs(left_Y_elbow_wrist_diff); right_Y_elbow_wrist_diff = Math.Abs(right_Y_elbow_wrist_diff); left_Y_shoul_wrist_diff = Math.Abs(left_Y_shoul_wrist_diff); right_Y_shoul_wrist_diff = Math.Abs(right_Y_shoul_wrist_diff); //TextInformation.insert_main_text_block("Y Shoulder Elbow Diff: " + Math.Abs(left_Y_shoul_elbow_diff - right_Y_shoul_elbow_diff).ToString("n5"), 4); //TextInformation.insert_main_text_block("Y Elbow Wrist Diff: " + Math.Abs(left_Y_shoul_elbow_diff - right_Y_shoul_elbow_diff).ToString("n5"), 4); // We need to see if the differences between the first two pairs (short limbs) are within a limit: if ((Math.Abs(left_Y_shoul_elbow_diff - right_Y_shoul_elbow_diff) < trigg_2_short_limb_diff_sensitivity) && (Math.Abs(left_Y_elbow_wrist_diff - right_Y_elbow_wrist_diff) < trigg_2_short_limb_diff_sensitivity)) { short_arm_limb_steady = true; } //TextInformation.insert_main_text_block("Y Long Diff: " + Math.Abs(left_Y_shoul_wrist_diff - right_Y_shoul_wrist_diff).ToString("n5"), 4); // We need to see if the differences between the last pair (short limb) is within a limit: if ((Math.Abs(left_Y_shoul_wrist_diff - right_Y_shoul_wrist_diff) < trigg_2_long_limb_diff_sensitivity)) { long_arm_limb_steady = true; } // We check the Z distances from the camera: double left_shoulder_Z = GesturesMasterControl.body.Joints[JointType.ShoulderLeft].Position.Z; double right_shoulder_Z = GesturesMasterControl.body.Joints[JointType.ShoulderRight].Position.Z; double left_elbow_Z = GesturesMasterControl.body.Joints[JointType.ElbowLeft].Position.Z; double right_elbow_Z = GesturesMasterControl.body.Joints[JointType.ElbowRight].Position.Z; double left_wrist_Z = GesturesMasterControl.body.Joints[JointType.WristLeft].Position.Z; double right_wrist_Z = GesturesMasterControl.body.Joints[JointType.WristRight].Position.Z; /*TextInformation.insert_main_text_block("Left Shoulder Z: " + left_shoulder_Z.ToString("n5"), 4); * TextInformation.insert_main_text_block("Right Shoulder Z: " + right_shoulder_Z.ToString("n5"), 4); * TextInformation.insert_main_text_block("Left Elbow Z: " + left_elbow_Z.ToString("n5"), 4); * TextInformation.insert_main_text_block("Right Shoulder Z: " + right_elbow_Z.ToString("n5"), 4); * TextInformation.insert_main_text_block("Left Wrist Z: " + left_wrist_Z.ToString("n5"), 4); * TextInformation.insert_main_text_block("Right Wrist Z: " + right_wrist_Z.ToString("n5"), 4); */ if ((Math.Abs(right_shoulder_Z - right_elbow_Z) < trigg_2_Z_short_dist_sensitivity) && (Math.Abs(right_elbow_Z - right_wrist_Z) < trigg_2_Z_short_dist_sensitivity) && (Math.Abs(right_shoulder_Z - right_wrist_Z) < trigg_2_Z_long_dist_sensitivity)) { right_Z_dist_steady = true; } if ((Math.Abs(left_shoulder_Z - left_elbow_Z) < trigg_2_Z_short_dist_sensitivity) && (Math.Abs(left_elbow_Z - left_wrist_Z) < trigg_2_Z_short_dist_sensitivity) && (Math.Abs(left_shoulder_Z - left_wrist_Z) < trigg_2_Z_long_dist_sensitivity)) { left_Z_dist_steady = true; } if (short_arm_limb_steady == true && long_arm_limb_steady == true && right_Z_dist_steady == true && left_Z_dist_steady == true) { return(true); } else { return(false); } }
private Class2.Vector4[] CreateCube(float scale, Class2.Vector4 position, float yaw, float pitch, float roll, ArmPosition position2) { //задаем координаты точек узлов робота drRobot = new Class2.Vector4[14]; //Создаем список сочленений float koef = 1.5F; drRobot[0] = new Class2.Vector4((float)0 * koef, (float)0 * koef, (float)0 * koef, 1); drRobot[1] = new Class2.Vector4((float)position2.X[0] * koef, (float)position2.Y[0] * koef, (float)position2.Z[0] * koef, 1); drRobot[2] = new Class2.Vector4((float)position2.X[1] * koef, (float)position2.Y[1] * koef, (float)position2.Z[1] * koef, 1); drRobot[3] = new Class2.Vector4((float)position2.X[2] * koef, (float)position2.Y[2] * koef, (float)position2.Z[2] * koef, 1); drRobot[4] = new Class2.Vector4((float)position2.X[3] * koef, (float)position2.Y[3] * koef, (float)position2.Z[3] * koef, 1); drRobot[5] = new Class2.Vector4((float)position2.X[4] * koef, (float)position2.Y[4] * koef, (float)position2.Z[4] * koef, 1); drRobot[6] = new Class2.Vector4((float)position2.X[5] * koef, (float)position2.Y[5] * koef, (float)position2.Z[5] * koef, 1); drRobot[7] = new Class2.Vector4((float)position2.X[6] * koef, (float)position2.Y[6] * koef, (float)position2.Z[6] * koef, 1); drRobot[8] = new Class2.Vector4((float)position2.X[7] * koef, (float)position2.Y[7] * koef, (float)position2.Z[7] * koef, 1); drRobot[9] = new Class2.Vector4((float)position2.X[8] * koef, (float)position2.Y[8] * koef, (float)position2.Z[8] * koef, 1); drRobot[10] = new Class2.Vector4((float)position2.X[9] * koef, (float)position2.Y[9] * koef, (float)position2.Z[9] * koef, 1); drRobot[11] = new Class2.Vector4((float)position2.X[10] * koef, (float)position2.Y[10] * koef, (float)position2.Z[10] * koef, 1); drRobot[12] = new Class2.Vector4((float)position2.X[11] * koef, (float)position2.Y[11] * koef, (float)position2.Z[11] * koef, 1); //матрица масштабирования var scaleM = Class2.Matrix4x4.CreateScale(scale / 2); //матрица вращения var rotateM = Class2.Matrix4x4.CreateFromYawPitchRoll(yaw, pitch, roll); //матрица переноса var translateM = Class2.Matrix4x4.CreateTranslation(position); //результирующая матрица var m = translateM * rotateM * scaleM; //умножаем векторы на матрицу for (int i = 0; i < drRobot.Length; i++) { drRobot[i] = m * drRobot[i]; } return(drRobot); }
private void OnNewArmPosition(ArmPosition position) => Commands.Add(new MoveCommand(position));