public Tuple <double, double, double> ToCoordinate(PosePosition pos)
{
var lowAngle = LowAngle(pos.X);
var highAngle = HighAngle(pos.X, pos.Y);
var lowRadian = AngleToRadian(lowAngle);
var highRadian = AngleToRadian(highAngle);
var alphaAngle = (180 - highAngle) / 2.0;
var alpha = AngleToRadian(alphaAngle);
var bottomL = BottomOfIsoscelesTriangle(l, alpha);
var beta = (Math.PI - alpha - lowRadian); //.RandWithFiveDigites();
var zCoord = (Math.Sin(beta) * bottomL); //.RandWithFiveDigites();
var length = (Math.Cos(beta) * bottomL); //.RandWithFiveDigites();
var rototeAngle = MmToAngle(pos.Z);
var rototeRadian = AngleToRadian(rototeAngle);
var xCoord = (Math.Cos(rototeRadian) * length); //.RandWithFiveDigites();
var yCoord = (Math.Sin(rototeRadian) * length); //.RandWithFiveDigites();
return(new Tuple <double, double, double>(xCoord, yCoord, zCoord));
}
/**
* Act on node movement
* param name="posePos" The new position value for this node
* param name="node" The node this new position is for
* param name="previousCoord" The previous coordinates for this node (this will be updated by the function to the new last values, that is the current)
* param name="desc" The description of the node (used for logging)
*/
private void handleNodeMovement(PosePosition posePos, GameObject node, ref Vector2 previousCoord, string desc)
{
if (node == null)
{
// GameObject not set -> ignore it
return;
}
Vector2 currentCoord = convertPercentageToPixels(posePos);
MaxMinCoord mmCoord = maxMinCoordMap[desc];
if (mmCoord != null)
{
mmCoord.handleCoord(currentCoord);
// Debug.Log(desc + ": " + mmCoord.ToString());
}
// Smoothen the change in controlled GameObject
float delta = currentCoord.x - previousCoord.x;
currentCoord.x = previousCoord.x + (delta / smoothening);
delta = currentCoord.y - previousCoord.y;
currentCoord.y = previousCoord.y + (delta / smoothening);
previousCoord.x = currentCoord.x;
previousCoord.y = currentCoord.y;
Transform transform = node.transform;
transform.localPosition = new Vector3(currentCoord.x, currentCoord.y, transform.localPosition.z);
}
/**
* Act on node movement
* param name="posePos" The new position value for this node
* param name="node" The node this new position is for
* param name="desc" The description of the node (used for logging)
*/
private void handleNodeMovement(PosePosition posePos, GameObject node, ref Vector2 previousCoord, string desc)
{
if (node == null)
{
// GameObject not set -> ignore it
return;
}
// Add offset
posePos.x = posePos.x - adjustmentToZero.x;
posePos.y = posePos.y - adjustmentToZero.y;
Vector2 currentCoord = convertPercentageToPixels(posePos);
if (node == leftFoot)
{
Debug.Log("currentCoord: " + currentCoord.ToString());
}
// Smoothen the change in controlled GameObject
float delta = currentCoord.x - previousCoord.x;
currentCoord.x = previousCoord.x + (delta / smoothening);
delta = currentCoord.y - previousCoord.y;
currentCoord.y = previousCoord.y + (delta / smoothening);
previousCoord.x = currentCoord.x;
previousCoord.y = currentCoord.y;
if (node == leftFoot)
{
Debug.Log("posePos: " + posePos.ToString() + ", previousCoord: " + previousCoord.ToString() + ", adjustmentToZero: " + adjustmentToZero.ToString());
}
Transform transform = node.transform;
transform.localPosition = new Vector3(previousCoord.x, previousCoord.y, transform.localPosition.z);
}
public MainWindow()
{
InitializeComponent();
//_testBrain = new TestRunner();
//_testBrain.RegisterTestTarget();
_deviceId = Guid.NewGuid();
_currentPosePosition = PosePosition.InitializePosition();
DataContext = _dataContext;
ComboBoxBaud.SetBinding(ItemsControl.ItemsSourceProperty, new Binding {
Source = _baudList
});
ComboBoxBaud.SelectedIndex = 5;
CommandExecutor.SharedInstance.LogHandler = ShowLog;
CommandExecutor.SharedInstance.TakePhoto = (string fileName) => {
Application.Current.Dispatcher.Invoke(() =>
{
// not thread safe
this.SaveImgFileName = fileName;
this.SaveAFrame = true;
});
};
}
/**
* Converts screen percentage to actual Unity screen pixels.
* param name="posePos" A position in %
* returns The position in game coordinates
*/
private Vector2 convertPercentageToPixels(PosePosition posePos)
{
Vector2 coord = new Vector2(0, 0);
coord.x = posePos.x * (avatarWidthFactor / 100.0f);
coord.y = posePos.y * (avatarHeightFactor / 100.0f); // (100 - adjustmentToZero.y));
return(coord);
}
public static PosePosition operator -(PosePosition a, PosePosition b)
{
PosePosition p = new PosePosition();
p.x = a.x - b.x;
p.y = a.y - b.y;
p.z = a.z - b.z;
return(p);
}
public void TestCoordToPose()
{
var initPose = new PosePosition(680, 1040, 2400);
var coord = ArmPositionCalculator.SharedInstance.ToCoordinate(new PosePosition(680, 1040, 2400));
var postPose = ArmPositionCalculator.SharedInstance.ToPose(coord);
Assert.IsNotNull(postPose);
Assert.IsTrue(Math.Abs(postPose.X - initPose.X) < Tolerance);
Assert.IsTrue(Math.Abs(postPose.Y - initPose.Y) < Tolerance);
Assert.IsTrue(Math.Abs(postPose.Z - initPose.Z) < Tolerance);
}
/**
* Converts screen percentage to actual Unity screen pixels.
* param name="posePos" A position in %
* returns The position in game coordinates
*/
private Vector2 convertPercentageToPixels(PosePosition posePos)
{
float x = posePos.x;
float y = posePos.y;
Vector2 coord = new Vector2(0, 0);
coord.x = x * (widthSizeFactor / 100);
coord.y = y * (heightSizeFactor / 100);
return(coord);
}
private void ControllerShowCurrentStatus(PosePosition pose)
{
CurrentPoseXLabel.Content = $"X:{pose.X}";
CurrentPoseYLabel.Content = $"Y:{pose.Y}";
CurrentPoseZLabel.Content = $"z:{pose.Z}";
var coor = controlArm.ConvertToCoordinate(pose);
CurrentCoordinateXLabel.Content = $"X:{coor.Item1}";
CurrentCoordinateYLabel.Content = $"Y:{coor.Item2}";
CurrentCoordinateZLabel.Content = $"Z:{coor.Item3}";
}
// Smoothen the change in controlled GameObject
private Vector2 smoothenMovement(PosePosition posePos, Vector2 previousCoord)
{
Vector2 newCoord = new Vector2(0, 0);
float delta = posePos.x - previousCoord.x;
newCoord.x = previousCoord.x + (delta / smoothening);
delta = posePos.y - previousCoord.y;
newCoord.y = previousCoord.y + (delta / smoothening);
// Debug.Log("New pos for '" + desc + "', " + " Before: [x:" + previousCoord.x + ", y: " + previousCoord.y + "] -> After: [x: " + currentCoord.x + ", y: " + currentCoord.y + "]");
return(newCoord);
}
private void AbsoluteGoBtn_Click(object sender, RoutedEventArgs e)
{
if (((controlArm != null) && controlArm.IsConnected))
{
var x = TextToInt(AbsoluteXTextBox.Text);
var y = TextToInt(AbsoluteYTextBox.Text);
var z = TextToInt(AbsoluteZTextBox.Text);
var pose = new PosePosition(x, y, z);
controlArm.MoveTo(pose);
}
}
private void RelativeGoBtn_Click(object sender, RoutedEventArgs e)
{
if (((controlArm != null) && controlArm.IsConnected))
{
var xInc = TextToInt(RelativeXTextBox.Text);
var yInc = TextToInt(RelativeYTextBox.Text);
var zInc = TextToInt(RelativeZTextBox.Text);
var currentPosition = controlArm.GetLatestData();
var targetPose = new PosePosition(currentPosition.X + xInc,
currentPosition.Y + yInc, currentPosition.Z + zInc);
controlArm.MoveTo(targetPose);
}
}
/**
* Called each frame to update the view
*/
void Update()
{
handleCameraDimensions();
if (lastPose != null)
{ // act on last pose-event
handleNodeMovement(lastPose.nose, nose, ref prevNoseCoord, "nose");
handleNodeMovement(lastPose.leftEye, leftEye, ref prevLeftEyeCoord, "leftEye");
handleNodeMovement(lastPose.rightEye, rightEye, ref prevRightEyeCoord, "rightEye");
handleNodeMovement(lastPose.leftEar, leftEar, ref prevLeftEarCoord, "leftEar");
handleNodeMovement(lastPose.rightEar, rightEar, ref prevRightEarCoord, "rightEar");
handleNodeMovement(lastPose.leftShoulder, leftShoulder, ref prevLeftShoulderCoord, "leftShoulder");
handleNodeMovement(lastPose.rightShoulder, rightShoulder, ref prevRightShoulderCoord, "rightShoulder");
handleNodeMovement(lastPose.leftElbow, leftElbow, ref prevLeftElbowCoord, "leftElbow");
handleNodeMovement(lastPose.rightElbow, rightElbow, ref prevRightElbowCoord, "rightElbow");
handleNodeMovement(lastPose.leftWrist, leftHand, ref prevLeftWristCoord, "leftHand");
handleNodeMovement(lastPose.rightWrist, rightHand, ref prevRightWristCoord, "rightHand");
handleNodeMovement(lastPose.leftHip, leftHip, ref prevLeftHipCoord, "leftHip");
handleNodeMovement(lastPose.rightHip, rightHip, ref prevRightHipCoord, "rightHip");
handleNodeMovement(lastPose.leftKnee, leftKnee, ref prevLeftKneeCoord, "leftKnee");
handleNodeMovement(lastPose.rightKnee, rightKnee, ref prevRightKneeCoord, "rightKnee");
handleNodeMovement(lastPose.leftFoot, leftFoot, ref prevLeftFootCoord, "leftFoot");
handleNodeMovement(lastPose.rightFoot, rightFoot, ref prevRightFootCoord, "rightFoot");
PosePosition pelvisPose = new PosePosition();
pelvisPose.x = (lastPose.rightHip.x + lastPose.leftHip.x) / 2;
pelvisPose.y = (lastPose.rightHip.y + lastPose.leftHip.y) / 2;
PosePosition middleSpinePose = new PosePosition();
middleSpinePose.x = (lastPose.rightHip.x + lastPose.leftShoulder.x) / 2;
middleSpinePose.y = (lastPose.leftShoulder.y + lastPose.leftHip.y) / 2;
handleNodeMovement(pelvisPose, root, ref prevRootCoord, "Root");
handleNodeMovement(middleSpinePose, middleSpine, ref prevMiddleSpineCoord, "MiddleSpine");
processedPose = lastPose;
lastPose = null;
}
else
{
// Debug.Log("No lastPose present");
}
if (drawSkeleton && skeleton != null)
{
skeleton.Update();
}
}
/**
* This is a hack to set all values missing in the received msg to null while the Unity JSON utility always creates an object for everything even if it is missing.
*/
public void setAllValuesWithoutValueToNull()
{
if (!root.isSet())
{
root = null;
}
if (!spine1.isSet())
{
spine1 = null;
}
if (!spine2.isSet())
{
spine2 = null;
}
if (!spine3.isSet())
{
spine3 = null;
}
if (!spine4.isSet())
{
spine4 = null;
}
if (!spine5.isSet())
{
spine5 = null;
}
if (!spine6.isSet())
{
spine6 = null;
}
if (!spine7.isSet())
{
spine7 = null;
}
if (!leftFoot.isSet())
{
leftFoot = null;
}
if (!rightFoot.isSet())
{
rightFoot = null;
}
}
/**
* Act on node movement
* posPos values are in %
* param name="posePos" The current position in %
* param name="node" The node this position is for
* param name="previousCoord" The previous coordinate
*/
private void handleHeadNodeMovement(PosePosition posePos, GameObject node, ref Vector2 previousCoord, string nodeName)
{
if (node == null)
{
// GameObject not set -> ignore it
return;
}
// Try to remove gittering in positions due to invalid points by smoothening
previousCoord = smoothenMovement(posePos, previousCoord);
// Convert from percentage value to game coordinates & adjust for screen center not being zero in input
Vector2 currentCoord = (previousCoord - prevLeftShoulderCoord) * headScaleAdjustment + currentLeftShoulderPos;
Debug.Log("New pos for '" + nodeName + "', " + " Pose: " + posePos + " -> After: " + currentCoord);
// Set new position on node
Transform transform = node.transform;
transform.localPosition = new Vector3(currentCoord.x, currentCoord.y, transform.localPosition.z);
}
/**
* Act on node movement
* posPos values are in %
* param name="posePos" The current position in %
* param name="node" The node this position is for
* param name="previousCoord" The previous coordinate
*/
private void handleNodeMovement(PosePosition posePos, GameObject node, ref Vector2 previousCoord, string nodeName, ref Vector2 currentPos)
{
if (node == null)
{
// GameObject not set -> ignore it
return;
}
// Try to remove gittering in positions due to invalid points by smoothening
previousCoord = smoothenMovement(posePos, previousCoord);
// Convert from percentage value to game coordinates & adjust for screen center not being zero in input
currentPos = (previousCoord - zeroPointAdjustment) * legScaleAdjustment;
Debug.Log("New pos for '" + nodeName + "', " + " Pose: " + posePos + " -> After: " + currentPos);
// Debug.Log("floorPercentageLevel: " + floorPercentageLevel + ", xAvgFactor: " + xAvgFactor);
// Set new position on node
Transform transform = node.transform;
transform.localPosition = new Vector3(currentPos.x, currentPos.y, transform.localPosition.z);
}
public void ExecuteNextLeftArmCommand()
{
lock (SyncRoot)
{
LeftArmCurrentCommand = LeftArmCommandList.Dequeue();
LeftArmCurrentStatus = Status.Executing;
}
switch (LeftArmCurrentCommand.Type)
{
case CommandType.Pose:
var command = LeftArmCurrentCommand as PoseCommand;
command.SendTimeStamp = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
LeftArm.MoveTo(new PosePosition()
{
MotorOneSteps = command.NextPosePosition.X, MotorTwoSteps = command.NextPosePosition.Y, MotorThreeSteps = command.NextPosePosition.Z
});
break;
case CommandType.GCode:
var gcommand = LeftArmCurrentCommand as GCommand;
gcommand.SendTimeStamp = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
var newPose = new PosePosition()
{
MotorOneSteps = LeftArm.CurrentPose.X + gcommand.XDelta,
MotorTwoSteps = LeftArm.CurrentPose.Y + gcommand.YDelta,
MotorThreeSteps = LeftArm.CurrentPose.Z + gcommand.ZDelta,
};
LeftArm.MoveTo(newPose);
break;
default:
lock (SyncRoot)
{
LeftArmCurrentCommand = null;
LeftArmCurrentStatus = Status.Idle;
}
break;
}
}
public void set(PoseEvent poseEvent)
{
this.nose = poseEvent.nose;
this.leftFoot = poseEvent.leftFoot;
this.rightFoot = poseEvent.rightFoot;
this.leftEar = poseEvent.leftEar;
this.rightEar = poseEvent.rightEar;
this.leftElbow = poseEvent.leftElbow;
this.rightElbow = poseEvent.rightElbow;
this.leftEye = poseEvent.leftEye;
this.rightEye = poseEvent.rightEye;
this.leftHip = poseEvent.leftHip;
this.rightHip = poseEvent.rightHip;
this.leftKnee = poseEvent.leftKnee;
this.rightKnee = poseEvent.rightKnee;
this.leftShoulder = poseEvent.leftShoulder;
this.rightShoulder = poseEvent.rightShoulder;
this.leftWrist = poseEvent.leftWrist;
this.rightWrist = poseEvent.rightWrist;
this.root = poseEvent.root;
this.spine3 = poseEvent.spine3;
}
public BodyPositionState(BodyPositionState state)
{
this.nose = new PosePosition(state.nose);
this.leftFoot = new PosePosition(state.leftFoot);
this.rightFoot = new PosePosition(state.rightFoot);
this.leftEar = new PosePosition(state.leftEar);
this.rightEar = new PosePosition(state.rightEar);
this.leftElbow = new PosePosition(state.leftElbow);
this.rightElbow = new PosePosition(state.rightElbow);
this.leftEye = new PosePosition(state.leftEye);
this.rightEye = new PosePosition(state.rightEye);
this.leftHip = new PosePosition(state.leftHip);
this.rightHip = new PosePosition(state.rightHip);
this.leftKnee = new PosePosition(state.leftKnee);
this.rightKnee = new PosePosition(state.rightKnee);
this.leftShoulder = new PosePosition(state.leftShoulder);
this.rightShoulder = new PosePosition(state.rightShoulder);
this.leftWrist = new PosePosition(state.leftWrist);
this.rightWrist = new PosePosition(state.rightWrist);
this.root = new PosePosition(state.root);
this.spine3 = new PosePosition(state.spine3);
}
public BodyPositionState()
{
this.nose = new PosePosition();
this.leftFoot = new PosePosition();
this.rightFoot = new PosePosition();
this.leftEar = new PosePosition();
this.rightEar = new PosePosition();
this.leftElbow = new PosePosition();
this.rightElbow = new PosePosition();
this.leftEye = new PosePosition();
this.rightEye = new PosePosition();
this.leftHip = new PosePosition();
this.rightHip = new PosePosition();
this.leftKnee = new PosePosition();
this.rightKnee = new PosePosition();
this.leftShoulder = new PosePosition();
this.rightShoulder = new PosePosition();
this.leftWrist = new PosePosition();
this.rightWrist = new PosePosition();
this.root = new PosePosition();
this.spine3 = new PosePosition();
}
private void ConnectButton_OnClick(object sender, RoutedEventArgs e)
{
if ((_serialPort != null) && _serialPort.IsConnected)
{
_serialPort.Dispose();
_serialPort = null;
_dataContext.AddOutput($"Disconnected");
Scroller.ScrollToBottom();
ConnectButton.Content = "Conntect";
_currentPosePosition = PosePosition.InitializePosition();
ShowCurrentPosition();
//_testBrain.UnRegisterTestAgent();
Title = Title.Substring(0, Title.Length - Title.LastIndexOf("-", StringComparison.Ordinal));
}
else
{
var portName = ComboBoxPort.SelectedValue.ToString();
var baud = int.Parse(ComboBoxBaud.SelectedValue.ToString());
_serialPort = new SerialCommunicator(portName, baud);
_serialPort.Connect();
_serialPort.StartRead(DataReceivedHandler);
if (_serialPort.IsConnected)
{
_dataContext.AddOutput($"Connected to {portName}");
Scroller.ScrollToBottom();
ConnectButton.Content = "Disconntect";
Title = Title + " - Connected";
CommandExecutor.SharedInstance.Register();
ShowLog($"Devic is registed as {CommandExecutor.SharedInstance.RegisterId}");
//_testBrain.RegisterTestAgent(_deviceId.ToString());
}
}
}
private void CommandComplete(PosePosition nextP, long sendTimeStamp)
{
// update current test agent pose position
if (nextP != null)
{
_currentPosePosition = nextP;
ShowCurrentPosition();
}
// report pose position
_testBrain.Arm.ReportPose(
CommandExecutor.SharedInstance.RegisterId.Value,
sendTimeStamp.ToString(),
_currentPosePosition.X,
_currentPosePosition.Y,
_currentPosePosition.Z);
CommandStore.SharedInstance.CurrentCommand = null;
lock (CommandExecutor.SharedInstance)
{
IsWaitingResponse = false;
}
}
public PoseCommand(int x, int y, int z) : this()
{
TargetPose = new PosePosition(x, y, z);
}
public TestAgent(string id)
{
Id = id;
CurrentPosition = PosePosition.InitializePosition();
}
public PosePosition(PosePosition v)
{
x = v.x;
y = v.y;
z = v.z;
}
public Tuple <double, double, double> ConvertPositionToCoordinat(PosePosition pose)
{
return(ArmPositionCalculator.SharedInstance.ToCoordinate(pose));
}
public void ExecuteNextCommand()
{
lock (SyncRoot)
{
CurrentCommand = CommandList.Dequeue();
CurrentStatus = Status.Executing;
}
switch (CurrentCommand.Type)
{
case CommandType.Pose:
var command = CurrentCommand as PoseCommand;
command.SendTimeStamp = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
Arm.MoveTo(new PosePosition()
{
MotorOneSteps = command.NextPosePosition.X, MotorTwoSteps = command.NextPosePosition.Y, MotorThreeSteps = command.NextPosePosition.Z
});
break;
case CommandType.GCode:
var gcommand = CurrentCommand as GCommand;
gcommand.SendTimeStamp = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
var newPose = new PosePosition()
{
MotorOneSteps = Arm.CurrentPose.X + gcommand.XDelta,
MotorTwoSteps = Arm.CurrentPose.Y + gcommand.YDelta,
MotorThreeSteps = Arm.CurrentPose.Z + gcommand.ZDelta,
};
Arm.MoveTo(newPose);
break;
case CommandType.Vision:
var vcommand = CurrentCommand as VisionCommand;
HandleVisionTask(vcommand);
break;
case CommandType.WaitingForVisionAnalyze:
// TODO: Check Server Status;
break;
case CommandType.WaitingProb:
lock (SyncRoot)
{
CurrentStatus = Status.WaitingProb;
}
break;
case CommandType.WaitingForTouch:
lock (SyncRoot)
{
CurrentStatus = Status.WaitingTouch;
}
break;
case CommandType.Pause:
lock (SyncRoot)
{
CurrentStatus = Status.Pause;
}
break;
case CommandType.Done:
lock (SyncRoot)
{
CurrentStatus = Status.TaskDone;
}
break;
default:
lock (SyncRoot)
{
CurrentCommand = null;
CurrentStatus = Status.Idle;
}
break;
}
}
public GCommand(int xD, int yD, int zD, PosePosition position) : this(xD, yD, zD)
{
CurrentPosePosition = position ?? PosePosition.InitializePosition();
}