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));
