void Update()
{
// visualization for single markers
if (type != MarkerType.multi)
{
if (confidenceColoBox != null)
{
confidenceColoBox.color = new Color(0, confidence, 0, 1);
}
if (confidenceTextBox != null)
{
confidenceTextBox.text = string.Format("{0:0.000}", confidence);
}
}
// visulization for all
if (target != null && visible)
{
if (applyRotation)
{
target.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(transMatrix);
}
if (applyTranslation)
{
target.transform.localPosition = ARUWPUtils.PositionFromMatrix(transMatrix);
// Debug.Log(TAG + ": Current local position " + target.transform.localPosition);
}
}
}
/// <summary>
/// This function is called by the associated ARUWPMarker.cs to update the pose of this
/// virtual object. When smoothing is enabled, the new pose will be filtered with current
/// pose using lerp. Big sudden change of 6-DOF pose will be prohibited. [public use]
/// </summary>
public void SetMatrix4x4(Matrix4x4 localToWorldMatrix)
{
Vector3 previousPosition = transform.localPosition;
Quaternion previousRotation = transform.localRotation;
Vector3 targetPosition = ARUWPUtils.PositionFromMatrix(localToWorldMatrix);
Quaternion targetRotation = ARUWPUtils.QuaternionFromMatrix(localToWorldMatrix);
if (!smoothing)
{
transform.localRotation = targetRotation;
transform.localPosition = targetPosition;
}
else
{
float positionDiff = Vector3.Distance(targetPosition, previousPosition);
float rotationDiff = Quaternion.Angle(targetRotation, previousRotation);
if (Mathf.Abs(positionDiff) < positionJumpThreshold && Mathf.Abs(rotationDiff) < rotationJumpThreshold)
{
transform.localRotation = Quaternion.Slerp(previousRotation, targetRotation, lerp);
transform.localPosition = Vector3.Lerp(previousPosition, targetPosition, lerp);
pendingPositionList.Clear();
pendingRotationList.Clear();
}
else
{
// maybe there is a jump
pendingPositionList.Add(targetPosition);
pendingRotationList.Add(targetRotation);
bool confirmJump = true;
if (pendingPositionList.Count > maxPendingList)
{
for (int i = 0; i < maxPendingList - 1; i++)
{
float tempPositionDiff = Vector3.Distance(pendingPositionList[pendingPositionList.Count - i - 1], pendingPositionList[pendingPositionList.Count - i - 2]);
float tempRotationDiff = Quaternion.Angle(pendingRotationList[pendingRotationList.Count - i - 1], pendingRotationList[pendingRotationList.Count - i - 2]);
if (Mathf.Abs(tempPositionDiff) > positionRecoverThreshold || Mathf.Abs(tempRotationDiff) > rotationRecoverThreshold)
{
confirmJump = false;
break;
}
}
if (confirmJump)
{
transform.localRotation = targetRotation;
transform.localPosition = targetPosition;
pendingPositionList.Clear();
pendingRotationList.Clear();
}
}
}
}
}
// latestMarkerTransMatrix now contains the latest marker pose in static world frame
private void processMarkerTrackingInfo()
{
// Interlocked.Exchange(ref latestMarkerTransMatrix, aruwpMarker.GetMarkerTransformationMatrix());
latestMarkerTransMatrix = aruwpMarker.GetMarkerTransformationMatrix();
// convert the transformation in hololens frame
// to transformation in the global frame
// because hololens can move in the world, it's frame is not fixed
dummyGameObject.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestMarkerTransMatrix);
dummyGameObject.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestMarkerTransMatrix);
latestMarkerTransMatrix = dummyGameObject.transform.localToWorldMatrix;
// Debug.Log("marker to user" + ARUWPUtils.PositionFromMatrix(latestMarkerTransMatrix));
// Debug.Log("marker to user orientation" + ARUWPUtils.QuaternionFromMatrix(latestMarkerTransMatrix).eulerAngles);
}
/// <summary>
/// Unity Monobehavior function: update tracking information of the current marker.
/// If magic function is enabled, then a distortion transformation is applied on top
/// of the tracking result. [internal use]
/// </summary>
private void Update()
{
if (signalTrackingUpdated && id != -1)
{
Interlocked.Exchange(ref latestTrackingInfo, _info);
if (latestTrackingInfo.visible)
{
latestTransMatrix = ARUWPUtils.ConvertARUWPFloatArrayToMatrix4x4(latestTrackingInfo.trans);
if (performMagicFunction)
{
MagicFunction();
}
if (target != null)
{
if (!anchoredToWorld)
{
if (applyRotation)
{
target.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestTransMatrix);
}
if (applyTranslation)
{
target.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestTransMatrix);
}
}
else
{
if (applyRotation)
{
dummyGameObject.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestTrackingInfo.locatableCameraToWorld * latestTransMatrix);
}
if (applyTranslation)
{
dummyGameObject.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestTrackingInfo.locatableCameraToWorld * latestTransMatrix);
}
latestPoseInWorldCoordinates = dummyGameObject.transform.localToWorldMatrix;
ARUWPUtils.SetMatrix4x4ToGameObject(ref target, latestPoseInWorldCoordinates);
}
}
}
}
signalTrackingUpdated = false;
}
/// <summary>
/// Unity Monobehavior function: update tracking information of the current marker.
/// If magic function is enabled, then a distortion transformation is applied on top
/// of the tracking result. [internal use]
/// </summary>
private void LateUpdate()
{
if (signalTrackingUpdated && id != -1)
{
Interlocked.Exchange(ref latestTrackingInfo, _info);
if (latestTrackingInfo.visible)
{
latestTransMatrix = ARUWPUtils.ConvertARUWPFloatArrayToMatrix4x4(latestTrackingInfo.trans);
// ARssistSi
latestTransMatrix = calibrationMatrix * latestTransMatrix;
if (target != null)
{
if (!anchoredToWorld)
{
if (applyRotation)
{
target.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestTransMatrix);
}
if (applyTranslation)
{
target.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestTransMatrix);
}
}
else
{
//if (applyRotation)
//{
// dummyGameObject.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestTransMatrix);
//}
//if (applyTranslation)
//{
// dummyGameObject.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestTransMatrix);
//}
ARUWPUtils.SetMatrix4x4ToGameObject(ref target, latestTrackingInfo.locatableCameraToWorld * latestTransMatrix);
//ARUWPUtils.SetMatrix4x4ToGameObject(ref target, dummyGameObject.transform.localToWorldMatrix);
}
}
}
}
signalTrackingUpdated = false;
}
void Update()
{
// visualization for single markers
if (type != MarkerType.multi)
{
if (confidenceColoBox != null)
{
confidenceColoBox.color = new Color(0, confidence, 0, 1);
}
if (confidenceTextBox != null)
{
confidenceTextBox.text = string.Format("{0:0.000}", confidence);
}
}
// visulization for all
if (target != null && visible)
{
if (!anchoredToWorld)
{
if (applyRotation)
{
target.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(transMatrix);
}
if (applyTranslation)
{
target.transform.localPosition = ARUWPUtils.PositionFromMatrix(transMatrix);
}
}
else
{
if (applyRotation)
{
dummyGameObject.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(transMatrix);
}
if (applyTranslation)
{
dummyGameObject.transform.localPosition = ARUWPUtils.PositionFromMatrix(transMatrix);
}
ARUWPUtils.SetMatrix4x4ToGameObject(ref target, dummyGameObject.transform.localToWorldMatrix);
}
}
}
private void updateUILocations()
{
Vector3 headPos = ARUWPUtils.PositionFromMatrix(robotPoseToHololensMatrices[TCPManager.TCPPackageConstants.HEAD]);
Vector3 buttonPosBase = headPos + new Vector3(0.2f, 0.2f, 0);
// put plannerToggleButton at buttonPosBase
plannerToggleButton.transform.position = buttonPosBase;
// showing cameraToggleButton to the right
cameraToggleButton.transform.position = buttonPosBase + new Vector3(0.1f, 0, 0);
// showing tfToggleButton to the right of camera button + x
tfToggleButton.transform.position = buttonPosBase + new Vector3(0.2f, 0, 0);
// showing the images from robot camera above head (y+)
if (!isShowingCameraView)
{
previewPlane.transform.parent.position = headPos + new Vector3(0, 0.5f, 0);
}
// put the warning sign to be to the left of gripper (x-)
Vector3 leftHandPos = ARUWPUtils.PositionFromMatrix(robotPoseToHololensMatrices[TCPManager.TCPPackageConstants.LEFT_HAND]);
warningSign.transform.position = leftHandPos + new Vector3(-0.15f, -0.1f, 0);
// put gripper button to be to the right of the left gripper +x, and higher +y
gripperButton.transform.position = leftHandPos + new Vector3(0.25f, 0.2f, 0);
//put the planner canvas in front of the robot -z (default position)
// if the planner canvas is opened, stop setting this location (allow canvas to be dragged)
if (!isShowingPlannerCanvas)
{
plannerCanvas.transform.position = headPos + new Vector3(0, 0, -0.3f);
}
if (!isShowingForceBar)
{
forceBarGraph.transform.position = leftHandPos + new Vector3(-0.1f, 0, 0);
}
}
public void TryPublishing(TimeSpan currentTime, double elapsedTimeInSeconds)
{
if (elapsedTimeInSeconds >= nextPublishTime && marker != null)
{
nextPublishTime = nextPublishTime + publishPeriod;
if (marker.GetMarkerVisibility())
{
Matrix4x4 latestPoseMatrix = marker.GetMarkerPoseInWorldCoordinateFrame();
Quaternion currentRotation = ARUWPUtils.QuaternionFromMatrix(latestPoseMatrix);
Vector3 currentPosition = ARUWPUtils.PositionFromMatrix(latestPoseMatrix);
#if NETFX_CORE
ThreadPool.RunAsync((MarkerPoseSendWork) => { SendPose(currentTime.Add(Timer.GetOffsetUTC()), currentRotation, currentPosition); });
#endif
}
else
{
Debug.Log("Marker not detected.");
}
}
}
private void visualizeForce()
{
// the robot base frame xyz direction is not what we what
adjustForceArrowSize(_left_gripper_info.force.x, AXIS_X);
adjustForceArrowSize(-_left_gripper_info.force.y, AXIS_Y);
adjustForceArrowSize(_left_gripper_info.force.z, AXIS_Z);
adjustForceArrowSize(_left_gripper_info.force.magnitude, AXIS_TOTAL);
Vector3 dir = left_gripper_force_target_x.transform.up * _left_gripper_info.force.x +
left_gripper_force_target_y.transform.up * -_left_gripper_info.force.y +
left_gripper_force_target_z.transform.up * _left_gripper_info.force.z;
Quaternion rot = Quaternion.LookRotation(dir);
// Debug.Log(rot.eulerAngles);
left_gripper_force_target_total.transform.rotation = rot;
// Quaternion rot = Quaternion.FromToRotation(-left_gripper_force_target_z.transform.up, dir);
// left_gripper_force_target_total.transform.localRotation = rot;
// prevent the force visual overlapping with the tf frame on the left hand
// set in down -y, and to the left -x
left_gripper_force_target.transform.localPosition = ARUWPUtils.PositionFromMatrix(robotPoseToHololensMatrices[TCPManager.TCPPackageConstants.LEFT_GRIPPER])
+ new Vector3(0.07f, -0.1f, 0);
left_gripper_force_target.transform.localRotation = _forceVisualOrientation;
}
/// <summary>
/// Unity Monobehavior function: update tracking information of the current marker.
/// If magic function is enabled, then a distortion transformation is applied on top
/// of the tracking result. [internal use]
/// </summary>
private void Update()
{
if (signalTrackingUpdated && id != -1)
{
Interlocked.Exchange(ref latestTrackingInfo, _info);
if (latestTrackingInfo.visible)
{
latestTransMatrix = ARUWPUtils.ConvertARUWPFloatArrayToMatrix4x4(latestTrackingInfo.trans);
//if (performMagicFunction) { MagicFunction(); }
if (target != null)
{
if (!anchoredToWorld)
{
if (applyRotation)
{
target.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestTransMatrix);
}
if (applyTranslation)
{
target.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestTransMatrix);
}
if (performCustomMagic)
{
Matrix4x4 newPos = new Matrix4x4();
newPos.SetColumn(0, new Vector4(target.transform.localPosition.x, dummyGameObject.transform.localPosition.y, dummyGameObject.transform.localPosition.z, 1));
Matrix4x4 result = newMagic * newPos;
target.transform.localPosition = result.GetColumn(0);
}
}
else
{
if (applyRotation)
{
dummyGameObject.transform.localRotation = ARUWPUtils.QuaternionFromMatrix(latestTransMatrix);
}
if (applyTranslation)
{
dummyGameObject.transform.localPosition = ARUWPUtils.PositionFromMatrix(latestTransMatrix);
}
if (performCustomMagic)
{
Matrix4x4 newPos = new Matrix4x4();
newPos.SetColumn(0, new Vector4(dummyGameObject.transform.localPosition.x, dummyGameObject.transform.localPosition.y, dummyGameObject.transform.localPosition.z, 1));
Matrix4x4 result = newMagic * newPos;
dummyGameObject.transform.localPosition = result.GetColumn(0);
}
// Dummy Object is anchored to camera. And this line converts it to world coordinate.
ARUWPUtils.SetMatrix4x4ToGameObject(ref target, dummyGameObject.transform.localToWorldMatrix);
//if (performCustomMagic)
//{
// Matrix4x4 newPos = new Matrix4x4();
// newPos.SetColumn(0, new Vector4(target.transform.localPosition.x, target.transform.localPosition.y, target.transform.localPosition.z, 1));
// Matrix4x4 result = newMagic * newPos;
// target.transform.localPosition = result.GetColumn(0);
//}
}
}
}
}
signalTrackingUpdated = false;
}