// Update is called once per frame
void Update()
{
// Code for adjusting height here.
// Currently supports ground only type
// First check height
float distanceRequired = distanceToHold + distanceToHoldOffset;
RaycastHit strikePoint;
int layerMask = 1 << 8;
if (vertexPoint != null)
{
// Find current distance to ground
// Needs to be upgraded to work with both types defined above.
if (Physics.Raycast(vertexPoint.transform.position, -Vector3.up, out strikePoint, 200.0f, layerMask))
{
projectedGroundPosition = strikePoint.point;
currentDistance = strikePoint.distance;
BMod_BalancePosition balPos = transform.parent.GetComponent <BMod_BalancePosition>(); // Hit and hope
if (balPos != null)
{
if (currentDistance != distanceRequired)
{
Vector3 current = vertexPoint.transform.position;
Vector3 shouldBe = new Vector3(current.x, distanceRequired, current.z);
Vector3 moveTo = Vector3.MoveTowards(current, shouldBe, balPos.maxSlideMovementSpeed * Time.deltaTime);
vertexPoint.offset += (moveTo - current); // Only apply translation.
}
}
}
}
}
public void setMaxLegLiftOnBalPols()
{
foreach (RFrame_Object frameObject in GetComponentsInChildren <RFrame_Object>())
{
BMod_BalancePosition balPol = frameObject.GetComponentInChildren <BMod_BalancePosition>();
balPol.maxReleasable = int.Parse(maxLegLiftBalPols.text);
}
}
public void setMaxSpeedOnBalPols()
{
foreach (RFrame_Object frameObject in GetComponentsInChildren <RFrame_Object>())
{
BMod_BalancePosition balPol = frameObject.GetComponentInChildren <BMod_BalancePosition>();
balPol.maxSlideMovementSpeed = float.Parse(maxSpeedOnBalPols.text);
}
}
// Update is called once per frame
void Update()
{
// Collect the angles from each angle link
bool ableToSend = true;
if (connectionForRobotSocket != null)
{
if (connectionForRobotSocket.Connected & Time.time > lastTime)
{
BMod_BalancePosition balPos = GetComponentInChildren <BMod_BalancePosition>();
if (balPos != null)
{
// Prevent the sending of movement commands until the balance positioner
// has completed its start-up routine.
if (balPos.balPosWorkingStatus == BMod_BalancePosition.balPolStatusEnum.startingUp)
{
ableToSend = false;
}
}
if (ableToSend)
{
lastTime = Time.time + networkSendWaitGap;
RFrame_AngleLink[] angleLinks = GetComponentsInChildren <RFrame_AngleLink>();
string messageToSend = "|servo";
anglesChanged = false;
foreach (RFrame_AngleLink link in angleLinks)
{
float angleToSend = Mathf.Round(link.channelSendAngle * 10.0f) / 10.0f;
if (previousAngles[link.linkChannel] != angleToSend)
{
anglesChanged = true;
previousAngles[link.linkChannel] = angleToSend;
}
// An end tag "*" is added after each angle to ensure complete angle values are applied.
// This is because during a network send, sometimes not all of the message is received,
// and sometimes a valid angle is reveived but it is truncated. When running this causes
// twitching.
// This isn't required for the channel number since if the message is truncated that
// far then there won't be any angle to apply. Plus the python code on the other end
// filters out any incomplete channel-angle pairs.
messageToSend = string.Concat(messageToSend, ",", link.linkChannel.ToString(), "-", angleToSend.ToString("0.00") + "*");
}
// Only send message if the angles have changed.
if (anglesChanged)
{
byte[] msg = System.Text.Encoding.ASCII.GetBytes(messageToSend);
connectionForRobotSocket.Send(msg);
}
}
}
}
}
/**
* Test method for creating a Frame_Object from an OBJ file
* 23 Jan 2019 - Updated to read from modified OBJ files.
* @param objFile The filename of the OBJ file
* @return A Frame_Object file
* @throws FileNotFoundException
*/
void createFromOBJ(TextAsset objFile)
{
//System.out.println("Loading OBJ file : " + objFile.toString());
string[] inFile = objFile.text.Split('\n');
List <GameObject> verticesBuild = new List <GameObject>();
List <GameObject> strutBuild = new List <GameObject>();
List <GameObject> angleViewBuild = new List <GameObject>();
List <GameObject> compRepBuild = new List <GameObject>();
List <GameObject> balPosBuild = new List <GameObject>();
List <GameObject> disHolBuild = new List <GameObject>();
List <GameObject> ancPoiBuild = new List <GameObject>();
int vertexIndex = 0;
foreach (string objLine in inFile)
{
//System.out.println(objLine); // Debug print to console the current line.
// Reading in a vertex line
if (objLine.StartsWith("v") && !objLine.StartsWith("vn") && !objLine.StartsWith("vt") && !objLine.StartsWith("vp"))
{
// Split the string using space. e.g. v 3.00 2.00 -2.00
//System.out.println("Vertex : " + objLine);
string[] splits = objLine.Split(' ');
float x = float.Parse(splits[1]);
float y = float.Parse(splits[2]);
float z = float.Parse(splits[3]);
GameObject vertexGameObject = Instantiate(VertexInstantiator, this.transform);
vertexGameObject.name = vertexGameObject.name + "_" + vertexIndex;
vertexGameObject.transform.localPosition = new Vector3(x, y, z);
RFrame_Vertex vertexScript = vertexGameObject.GetComponent <RFrame_Vertex>();
vertexScript.x = x;
vertexScript.y = y;
vertexScript.z = z;
// Check for lock character
if (splits.Length > 4)
{
if (splits[4] == "l")
{
vertexScript.lockedInPlace = true;
}
}
verticesBuild.Add(vertexGameObject);
vertexIndex++;
}
else if (objLine.StartsWith("l"))
{
// This is for any standalone lines, which will be turned into struts.
// Modified to only work with the first two vertice indexes
string[] splits = objLine.Split(' ');
// Remember that indexes start at 1 in the obj/txt file and need converting to zero-indexed.
int v1 = int.Parse(splits[1]) - 1;
int v2 = int.Parse(splits[2]) - 1;
GameObject strutGameObject = RFrame_Object.createStrutIfNotExisting(this, StrutInstantiator, strutBuild, v1, v2);
// Check if strut is displayed. This is determined by having more than three entries in splits.
// Basically any character entered after the 2 indexes will trigger this.
//if(strutGameObject!=null && splits.Length > 3) {
if (splits.Length == 5)
{
RFrame_Strut strutScript = strutGameObject.GetComponent <RFrame_Strut>();
strutScript.minLength = float.Parse(splits[3]);
strutScript.maxLength = float.Parse(splits[4]);
}
// All line renderers will be switched off.
LineRenderer lineRenderer = strutGameObject.GetComponent <LineRenderer>();
// Used when debugging the a fresh OBJ frame.
lineRenderer.enabled = transform.GetComponentInParent <RFrame_Bridge>().displayStrutsOnLoad;
//}
}
else if (objLine.StartsWith("anglink"))
{
// Angle link entry.
// Requires 4 vertex indexes and a link channel number which will represent the angle measurer struts.
// and the actuator/motor/servo this is linked too.
// Centering and angle limits should be entered.
string[] splits = objLine.Split(' ');
int centerVertex = int.Parse(splits[1]);
int upVertex = int.Parse(splits[2]);
int measure1 = int.Parse(splits[3]);
int measure2 = int.Parse(splits[4]);
int channel = int.Parse(splits[5]); // Channel number should already be zero-indexed.
bool invertChannel = bool.Parse(splits[6]); // Should be 0, 1, true or false
float minChannelAngle = float.Parse(splits[7]); // Channel won't be sent any number smaller than this.
float maxChannelAngle = float.Parse(splits[8]); // Channel won't be sent any number larger than this.
float biasAngle = float.Parse(splits[9]); // The bias to apply. Indicates that a servo may need recentering
int angleMeasureType = int.Parse(splits[10]); // Quaternion or dot product angle
float scaleChangedAngled = float.Parse(splits[11]); // How much to scale the change in angle from initial. Increases range of movement.
GameObject angleViewerObj = Instantiate(AngleViewerInstantiator, this.transform);
RFrame_AngleLink angleLinker = angleViewerObj.GetComponent <RFrame_AngleLink>();
angleLinker.centerVertex = centerVertex;
angleLinker.upVertex = upVertex;
angleLinker.measure1 = measure1;
angleLinker.measure2 = measure2;
angleLinker.linkChannel = channel;
angleLinker.invertedSendAngle = invertChannel;
angleLinker.minimumAngle = minChannelAngle;
angleLinker.maximumAngle = maxChannelAngle;
angleLinker.biasAngle = biasAngle;
angleLinker.angleChangeScaling = scaleChangedAngled;
switch (angleMeasureType)
{
case 0: {
angleLinker.angleMeasureType = RFrame_AngleLink.AngleMeasureTypeEnum.quaternionBased;
break;
}
case 1: {
angleLinker.angleMeasureType = RFrame_AngleLink.AngleMeasureTypeEnum.lineBased;
break;
}
default: {
angleLinker.angleMeasureType = RFrame_AngleLink.AngleMeasureTypeEnum.quaternionBased;
break;
}
}
angleViewBuild.Add(angleViewerObj);
}
else if (objLine.StartsWith("comprep") && transform.GetComponentInParent <RFrame_Bridge>().displayComponentRepsOnLoad)
{
// Component representation
string[] splits = objLine.Split(' ');
int centerVertex = int.Parse(splits[1]);
int upVertex = int.Parse(splits[2]);
int forwardVertex = int.Parse(splits[3]);
string meshName = splits[4];
float xRot = float.Parse(splits[5]);
float yRot = float.Parse(splits[6]);
float zRot = float.Parse(splits[7]);
float xTran = float.Parse(splits[8]);
float yTran = float.Parse(splits[9]);
float zTran = float.Parse(splits[10]);
GameObject compRepObj = Instantiate(ComponentRepresentorInstantiator, this.transform);
RFrame_Component componentRep = compRepObj.GetComponent <RFrame_Component>();
componentRep.centerVertex = centerVertex;
componentRep.forwardVertex = forwardVertex;
componentRep.upVertex = upVertex;
GameObject meshObject = Instantiate(Resources.Load <GameObject>(meshName), compRepObj.transform);
meshObject.transform.localPosition = new Vector3(xTran, yTran, zTran);
meshObject.transform.localRotation = Quaternion.Euler(xRot, yRot, zRot);
}
else if (objLine.StartsWith("balpos"))
{
// Balance position behaviour module building here
string[] splits = objLine.Split(' ');
int vertexAttachIndex = int.Parse(splits[1]);
float reduceRatio = float.Parse(splits[2]);
GameObject balPolObj = Instantiate(BalancePositionInstantiator, this.transform);
BMod_BalancePosition balPosMod = balPolObj.GetComponent <BMod_BalancePosition>();
balPosMod.restrictionAreaShrinkRatio = reduceRatio;
balPosMod.controlVertex = verticesBuild[vertexAttachIndex].GetComponent <RFrame_Vertex>();
//balPosMod.controlVertex.lockedInPlace = true;
balPosBuild.Add(balPolObj);
}
else if (objLine.StartsWith("dishol"))
{
// Distance holder behaviour module building here
string[] splits = objLine.Split(' ');
int balPosIndex = int.Parse(splits[1]);
int vertexAttachIndex = int.Parse(splits[2]);
float distanceToHold = float.Parse(splits[3]);
int typeOfDisHol = int.Parse(splits[4]);
GameObject disHolObj = Instantiate(DistanceHolderInstantiator, balPosBuild[balPosIndex].transform);
BMod_DistanceHolder disHolMod = disHolObj.GetComponent <BMod_DistanceHolder>();
disHolMod.vertexPoint = verticesBuild[vertexAttachIndex].GetComponent <RFrame_Vertex>();
// Experimenting with not locking the vertex in place.
// Need to make sure frame doesn't move out of place.
disHolMod.vertexPoint.lockedInPlace = true;
disHolMod.distanceToHold = distanceToHold;
switch (typeOfDisHol)
{
case 0: disHolMod.moduleType = BMod_DistanceHolder.DistanceModuleType.groundDistance; break;
case 1: disHolMod.moduleType = BMod_DistanceHolder.DistanceModuleType.allRoundDistance; break;
default: disHolMod.moduleType = BMod_DistanceHolder.DistanceModuleType.groundDistance; break;
}
disHolBuild.Add(disHolObj);
}
else if (objLine.StartsWith("ancpoi"))
{
// Anchor Point behaviour module building here
string[] splits = objLine.Split(' ');
int disHolIndex = int.Parse(splits[1]);
int vertexAttachIndex = int.Parse(splits[2]);
int priorityValue = int.Parse(splits[3]);
int typeOfAnchor = int.Parse(splits[4]);
GameObject anchorObj = Instantiate(AnchorPointInstantiator, disHolBuild[disHolIndex].transform);
BMod_AnchorPoint anchorMod = anchorObj.GetComponent <BMod_AnchorPoint>();
anchorMod.anchorVertex = verticesBuild[vertexAttachIndex].GetComponent <RFrame_Vertex>();
anchorMod.anchorVertex.lockedInPlace = true;
anchorMod.priorityIndex = priorityValue;
switch (typeOfAnchor)
{
case 0: anchorMod.anchorType = BMod_AnchorPoint.AnchorTypeEnum.primaryAnchor; break;
case 1: anchorMod.anchorType = BMod_AnchorPoint.AnchorTypeEnum.secondaryAnchor; break;
case 2: anchorMod.anchorType = BMod_AnchorPoint.AnchorTypeEnum.tertiaryAnchor; break;
default: anchorMod.anchorType = BMod_AnchorPoint.AnchorTypeEnum.primaryAnchor; break;
}
ancPoiBuild.Add(anchorObj);
}
}
// Set the parents of the objects instatiated to this object and
// call the function for calculating all the distances between
// struts.
if (verticesBuild.Count != 0 && strutBuild.Count != 0)
{
// Apply the created vertices and struts to this object.
// Set the parent of the vertices and struts to the transform of this frame object.
// foreach (GameObject vertex in verticesBuild) {
// vertex.transform.SetParent(this.transform);
// }
// foreach (GameObject strut in strutBuild) {
// strut.transform.SetParent(this.transform);
// }
allVertices = verticesBuild;
struts = strutBuild;
angleLinks = angleViewBuild;
compReps = compRepBuild;
this.calculateDistancesThenOrientate();
// Get the initial measured angles for each angle link.
foreach (GameObject angleLinkObj in angleViewBuild)
{
angleLinkObj.GetComponent <RFrame_AngleLink>().measureAngle(true);
}
//frame.gatherBounds();
}
}
void checkPositionStatus()
{
// Method for checking where this anchor point is in relation to where it should be.
switch (currentAction)
{
case AnchorCurrentActionEnum.idle: {
// Check if slide position has moved out of bounds of the distance ellipse.
BMod_BalancePosition balPos = transform.parent.parent.GetComponent <BMod_BalancePosition>(); // Hit and hope
if (balPos != null)
{
if (balPos.balPosWorkingStatus == BMod_BalancePosition.balPolStatusEnum.readyForWork)
{
Vector3 fromLockedToSliding = lockedPosition - anchorVertex.transform.position;
Vector3 fromLastDropPoint = dropPosition - anchorVertex.transform.position;
float angleFromLeft = Vector3.Angle(fromLockedToSliding, Vector3.left);
float distanceForAngle = disconnectDistanceForAngle(angleFromLeft);
// Apply ground pushing bias based on distance from locked to current
// float limitDist = limitDistanceForAngle(angleFromLeft);
// float ratioToLimit = 0.0f;
// if (fromLockedToSliding.magnitude==0.0f) {
// ratioToLimit = 1.0f;
// } else {
// ratioToLimit = 1.0f - (fromLockedToSliding.magnitude / limitDist);
// //Debug.Log(limitDist + " Limit distance, " + fromLockedToSliding.magnitude + " Mag, Limit distance ratio " + ratioToLimit);
// }
// if (fromLockedToSliding.magnitude > limitDist) {
// ratioToLimit = 0.0f;
// }
// float groundPush = pushDownDistance * balPos.noncumulativeOffset.magnitude * ratioToLimit;
// Vector3 newPos = new Vector3(anchorVertex.transform.position.x, -groundPush, anchorVertex.transform.position.z);
// anchorVertex.transform.position = newPos;
// //anchorVertex.x = anchorVertex.transform.localPosition.x;
// anchorVertex.y = anchorVertex.transform.localPosition.y;
//anchorVertex.z = anchorVertex.transform.localPosition.z;
//Debug.Log("Anchor " + priorityIndex + " angle " + angleFromLeft + " distance " + distanceForAngle);
//float xDebug = lockedPosition.x + 2.0f * Mathf.Cos(angleFromLeft*Mathf.Deg2Rad);
//float yDebug = lockedPosition.y + 0.3f;
//float zDebug = lockedPosition.z + 2.0f * Mathf.Sin(angleFromLeft * Mathf.Deg2Rad);
// Debug.DrawLine(lockedPosition, new Vector3(xDebug, yDebug, zDebug));
// Need to take into account that last drop position will be outside the last drop point.
// This codes needs to be more robust.
// MAYBE have unlock area change to encompass drop point.
// Attempting to use a standard deviation threshold is drop point and lock
// points differ.
if (lockedPosition != dropPosition)
{
float ld = (dropPosition - lockedPosition).magnitude;
float cl = (lockedPosition - anchorVertex.transform.position).magnitude;
float clcd = (dropPosition - anchorVertex.transform.position).magnitude + cl;
float average = (ld + clcd) / 2.0f;
float dev = Mathf.Sqrt((Mathf.Pow(ld - average, 2) + Mathf.Pow(clcd - average, 2)) / 2);
//Debug.Log("Deviation " + dev);
if (dev > balPos.lockDropDeviation)
{
needsToLift = true;
}
else
{
needsToLift = false;
}
}
else
{
if (fromLockedToSliding.magnitude > distanceForAngle)
{
needsToLift = true;
}
else
{
// This will happen in case anchor points which haven't lifted yet no longer need to lift.
needsToLift = false;
}
}
}
}
break;
}
case AnchorCurrentActionEnum.movingToStartingLockPosition: {
// 1 - Collect bias acceleration and apply to movement of vertex
// 2 - Check if destination reached
BMod_BalancePosition balPos = transform.parent.parent.GetComponent <BMod_BalancePosition>(); // Hit and hope
if (balPos != null)
{
//if (balPos.readyForWork) {
Vector3 newPos = Vector3.MoveTowards(anchorVertex.transform.position, lockedPosition, balPos.maxSlideMovementSpeed * Time.deltaTime);
anchorVertex.transform.position = newPos;
anchorVertex.x = anchorVertex.transform.localPosition.x;
anchorVertex.y = anchorVertex.transform.localPosition.y;
anchorVertex.z = anchorVertex.transform.localPosition.z;
// Check if destination has been reached
if (Vector3.Distance(newPos, lockedPosition) == 0.0f)
{
lockedInPosition = true;
currentAction = AnchorCurrentActionEnum.idle;
}
//}
}
break;
}
case AnchorCurrentActionEnum.movingToUnlockPosition: {
// Collect bias acceleration and apply to movement of vertex
// Check if destination reached
BMod_BalancePosition balPos = transform.parent.parent.GetComponent <BMod_BalancePosition>(); // Hit and hope
if (balPos != null)
{
if (balPos.balPosWorkingStatus == BMod_BalancePosition.balPolStatusEnum.readyForWork)
{
Vector3 newPos = Vector3.MoveTowards(anchorVertex.transform.position, unlockedPosition, balPos.maxSlideMovementSpeed * Time.deltaTime);
anchorVertex.transform.position = newPos;
anchorVertex.x = anchorVertex.transform.localPosition.x;
anchorVertex.y = anchorVertex.transform.localPosition.y;
anchorVertex.z = anchorVertex.transform.localPosition.z;
// Check if destination has been reached
if (Vector3.Distance(newPos, unlockedPosition) == 0.0f)
{
currentAction = AnchorCurrentActionEnum.movingToDesinationPosition;
}
}
}
break;
}
case AnchorCurrentActionEnum.movingToDesinationPosition: {
// Collect bias acceleration and apply to movement of vertex
// Check if destination reached
// The drop position is dynamic and linked to the direction of the main offset & rotation of the balance positioner
BMod_BalancePosition balPos = transform.parent.parent.GetComponent <BMod_BalancePosition>(); // Hit and hope
if (balPos != null)
{
if (balPos.balPosWorkingStatus == BMod_BalancePosition.balPolStatusEnum.readyForWork)
{
// Update the drop position based on the offset direction of the balance positioner and
// magnitude of non-cumulative offset. It is still slid around by the overall offset
Vector3 offsetDirection = balPos.noncumulativeOffset.normalized; // balPos.slideOffset.normalized;
// Swap the x and z around
float angleFromLeft = Vector3.Angle(offsetDirection, Vector3.left);
float distanceForAngle = disconnectDistanceForAngle(angleFromLeft);
float dropDistance = balPos.noncumulativeOffset.magnitude * (distanceForAngle * balPos.maxStepDistance);
dropPosition = lockedPosition + (-offsetDirection * dropDistance);
// Update the position of any drop representors.
if (dropPositionRepresentation != null)
{
dropPositionRepresentation.transform.position = dropPosition;
}
// Move delta distance to destination. The move speed is based on the noncumulative speed.
float noncumulativeMag = balPos.noncumulativeOffset.magnitude;
if (noncumulativeMag == 0.0f)
{
noncumulativeMag = 1.0f;
}
//float amountToMove = noncumulativeMag * balPos.maxSlideMovementSpeed*Time.deltaTime;
// Legs should move faster than slide speed
float amountToMove = 2.0f * balPos.maxSlideMovementSpeed * Time.deltaTime;
Vector3 newPos = Vector3.MoveTowards(anchorVertex.transform.position, dropPosition, amountToMove);
// Drop distance for the level with drop position
Vector3 newPosLevel = new Vector3(newPos.x, dropPosition.y, newPos.z);
// Interim code. Keep leg lifted until close to drop point.
// Updated so the leg is held high if it's over a certain distance away form the lock point.
float lockToDropDist = Vector3.Distance(lockedPosition, dropPosition);
float levelToDropDist = Vector3.Distance(newPosLevel, dropPosition);
if (levelToDropDist > lockToDropDist * lockDropHeightKeepRatio && lockToDropDist != 0.0f)
{
newPos.y = balPos.anchorLiftHeight;
}
// float xDebug = lockedPosition.x + 2.0f * Mathf.Cos(angleFromLeft * Mathf.Deg2Rad);
// float yDebug = lockedPosition.y + 0.3f;
// float zDebug = lockedPosition.z + 2.0f * Mathf.Sin(angleFromLeft * Mathf.Deg2Rad);
// Debug.DrawLine(lockedPosition, new Vector3(xDebug, yDebug, zDebug));
anchorVertex.transform.position = newPos;
anchorVertex.x = anchorVertex.transform.localPosition.x;
anchorVertex.y = anchorVertex.transform.localPosition.y;
anchorVertex.z = anchorVertex.transform.localPosition.z;
// Check if destination has been reached within a threshold value
float distanceToDrop = Vector3.Distance(newPos, dropPosition);
float threshold = balPos.dropDistanceThreshold;
if (distanceToDrop <= threshold)
{
currentAction = AnchorCurrentActionEnum.idle;
//lockedPosition = dropPosition;
lockedInPosition = true;
completedLockingSignal = true;
}
}
}
break;
}
case AnchorCurrentActionEnum.waitingForBalance: {
// This is for a leg has been given approval for lifting, but is waiting for balance to be reached.
BMod_BalancePosition balPos = transform.parent.parent.GetComponent <BMod_BalancePosition>(); // Hit and hope
if (balPos != null)
{
// If the balance hsa been reached then this anchor can be unlocked to move to unlock position.
if (balPos.balPosWorkingStatus == BMod_BalancePosition.balPolStatusEnum.readyForWork)
{
lockedInPosition = false;
currentAction = AnchorCurrentActionEnum.movingToUnlockPosition;
}
}
break;
}
}
}
