bool LoadRobot(string directory)
{
robotObject = new GameObject("Robot");
robotObject.transform.position = robotStartPosition;
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new RigidNode(guid));
};
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
rootNode = BXDJSkeleton.ReadSkeleton(directory + "\\skeleton.bxdj");
rootNode.ListAllNodes(nodes);
foreach (RigidNode_Base n in nodes)
{
RigidNode node = (RigidNode)n;
node.CreateTransform(robotObject.transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName))
{
Debug.Log("Robot not loaded!");
Destroy(robotObject);
return(false);
}
node.CreateJoint();
}
return(true);
}
public static void SetSolenoid(RigidNode node, bool forward)
{
float acceleration = 0;
B6DOFConstraint b6DOFConstraint = node.GetJoint <B6DOFConstraint>();
if (b6DOFConstraint == null)
{
return;
}
// TODO: This code is untested - test it.
if (b6DOFConstraint.motorLinearMaxMotorForce.x > 0)
{
acceleration = b6DOFConstraint.motorLinearMaxMotorForce.x / b6DOFConstraint.thisRigidBody.mass * (forward ? 1 : -1);
}
else
{
// TODO: Wth are all these arbitrary numbers??? Make constants.
float psiToNMm2 = 0.00689475728f;
float maximumForce = (psiToNMm2 * 60f) * (Mathf.PI * Mathf.Pow(6.35f, 2f));
acceleration = (maximumForce / b6DOFConstraint.thisRigidBody.mass) * (forward ? 1 : -1);
return;
}
// This is sketchy as heck, could be the cause of any issues that might occur.
float velocity = acceleration * (Time.deltaTime) - Vector3.Dot(b6DOFConstraint.thisRigidBody.velocity,
((RigidBody)node.MainObject.GetComponent <BRigidBody>().GetCollisionObject()).WorldTransform.ToUnity().MultiplyVector(b6DOFConstraint.localConstraintAxisX));
b6DOFConstraint.motorLinearTargetVelocity = new Vector3(velocity, 0f, 0f);
}
/// <summary>
/// Constructs the robot from the given list of nodes and number of wheels,
/// and updates the collective mass.
/// </summary>
/// <param name="nodes"></param>
/// <param name="numWheels"></param>
/// <param name="collectiveMass"></param>
/// <returns></returns>
protected virtual bool ConstructRobot(List <RigidNode_Base> nodes, ref float collectiveMass)
{
//Initializes the nodes
foreach (RigidNode_Base n in nodes)
{
RigidNode node = (RigidNode)n;
node.CreateTransform(transform);
if (!node.CreateMesh(RobotDirectory + "\\" + node.ModelFileName))
{
return(false);
}
if (node.PhysicalProperties != null)
{
collectiveMass += node.PhysicalProperties.mass;
}
}
RootNode.GenerateWheelInfo();
foreach (RigidNode_Base n in nodes)
{
((RigidNode)n).CreateJoint(this);
}
return(true);
}
/// <summary>
/// Creates a wheel and attaches it to the parent BRaycastVehicle.
/// </summary>
/// <param name="node"></param>
public void CreateWheel(RigidNode node)
{
this.node = node;
RigidNode parent = (RigidNode)node.GetParent();
robot = parent.MainObject.GetComponent <BRaycastRobot>();
if (robot == null)
{
Debug.LogError("Could not add BRaycastWheel because its parent does not have a BRaycastVehicle!");
Destroy(this);
}
RotationalJoint_Base joint = (RotationalJoint_Base)node.GetSkeletalJoint();
joint.basePoint.x *= -1;
node.OrientWheelNormals();
axis = joint.axis.AsV3();
WheelDriverMeta driverMeta = node.GetDriverMeta <WheelDriverMeta>();
radius = driverMeta.radius * 0.01f;
Vector3 localPosition = parent.MainObject.transform.InverseTransformPoint(node.MainObject.transform.position);
basePoint = localPosition.ToBullet() + new BulletSharp.Math.Vector3(0f, VerticalOffset, 0f);
wheelIndex = robot.AddWheel(driverMeta.type, basePoint, axis.normalized.ToBullet(), VerticalOffset, radius);
}
/// <summary>
/// Updates all emulation sensor values.
/// </summary>
/// <param name="emuList"></param>
private static void UpdateEmulationSensors(List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList)
{
int iter = 0;
foreach (Synthesis.Robot.RobotBase.EmuNetworkInfo a in emuList)
{
if (a.RobotSensor.type == RobotSensorType.ENCODER) // TODO revisit this
{
RigidNode rigidNode = null;
try
{
rigidNode = (RigidNode)(a.wheel);
}
catch (Exception e)
{
Debug.Log(e.StackTrace);
}
BRaycastWheel bRaycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
//BRaycastRobot robot = rigidNode.MainObject.GetComponent<BRaycastRobot>();
double wheelRadius = 3 / 39.3701;// robot.RaycastRobot.GetWheelInfo(0).WheelsRadius
Vector3 currentPos = bRaycastWheel.transform.position;
//double displacement = (Math.Sqrt(currentPos.x * currentPos.x + currentPos.z * currentPos.z) - (Math.Sqrt(a.previousPosition.x*a.previousPosition.x + a.previousPosition.z * a.previousPosition.z)));
double displacement = ((currentPos - a.previousPosition).magnitude) * Math.Sign(bRaycastWheel.GetWheelSpeed());
double angleDisplacement = (displacement) / (2 * 3.1415 * wheelRadius);
a.encoderTickCount += angleDisplacement * a.RobotSensor.conversionFactor;
a.previousPosition = currentPos;
if (Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter] == null)
{
Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter] = new EmulationService.RobotInputs.Types.EncoderManager();
}
EmulationService.RobotInputs.Types.EncoderManager.Types.PortType ConvertPortType(SensorConnectionType type)
{
if (type == SensorConnectionType.DIO)
{
return(EmulationService.RobotInputs.Types.EncoderManager.Types.PortType.Di);
}
if (type == SensorConnectionType.ANALOG)
{
return(EmulationService.RobotInputs.Types.EncoderManager.Types.PortType.Ai);
}
throw new Exception();
}
Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].AChannel = (uint)a.RobotSensor.portA;
Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].AType = ConvertPortType(a.RobotSensor.conTypePortA);;
Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].BChannel = (uint)a.RobotSensor.portB;
Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].BType = ConvertPortType(a.RobotSensor.conTypePortB);;
Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].Ticks = (int)a.encoderTickCount;
iter++;
}
}
}
/// <summary>
/// The lightweight equivalent of the 'Add From Inventor' button in the <see cref="ExporterForm"/>. Used in <see cref="ExportMeshesLite(RigidNode_Base)"/>
/// </summary>
/// <param name="occurrences"></param>
/// <returns></returns>
public RigidNode_Base ExportSkeleteonLite(List <ComponentOccurrence> occurrences)
{
if (occurrences.Count == 0)
{
throw new ArgumentException("ERROR: 0 Occurrences passed to ExportSkeletonLite", "occurrences");
}
#region CenterJoints
int NumCentered = 0;
SetProgressText(string.Format("Centering Joints {0} / {1}", NumCentered, occurrences.Count));
foreach (ComponentOccurrence component in occurrences)
{
Exporter.CenterAllJoints(component);
NumCentered++;
SetProgressText(string.Format("Centering Joints {0} / {1}", NumCentered, occurrences.Count));
}
#endregion
#region Build Models
//Getting Rigid Body Info...
SetProgressText("Getting Rigid Body Info...", ProgressTextType.ShortTaskBegin);
NameValueMap RigidGetOptions = InventorManager.Instance.TransientObjects.CreateNameValueMap();
RigidGetOptions.Add("DoubleBearing", false);
RigidBodyResults RawRigidResults = InventorManager.Instance.AssemblyDocument.ComponentDefinition.RigidBodyAnalysis(RigidGetOptions);
//Getting Rigid Body Info...Done
SetProgressText(null, ProgressTextType.ShortTaskEnd);
CustomRigidResults RigidResults = new CustomRigidResults(RawRigidResults);
//Building Model...
SetProgressText("Building Model...", ProgressTextType.ShortTaskBegin);
RigidBodyCleaner.CleanGroundedBodies(RigidResults);
RigidNode baseNode = RigidBodyCleaner.BuildAndCleanDijkstra(RigidResults);
//Building Model...Done
SetProgressText(null, ProgressTextType.ShortTaskEnd);
#endregion
#region Cleaning Up
//Cleaning Up...
LiteExporterForm.Instance.SetProgressText("Cleaning Up...", ProgressTextType.ShortTaskBegin);
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
baseNode.ListAllNodes(nodes);
foreach (RigidNode_Base node in nodes)
{
node.ModelFileName = ((RigidNode)node).group.ToString();
node.ModelFullID = node.GetModelID();
}
//Cleaning Up...Done
LiteExporterForm.Instance.SetProgressText(null, ProgressTextType.ShortTaskEnd);
#endregion
return(baseNode);
}
// TODO: Something weird is going on with the spawn, at least with robots with manipulators. Reset is fine.
/// <summary>
/// Loads and initializes the physical manipulator object (used in Mix and Match mode)
/// </summary>
/// <param name="directory">Folder directory of the manipulator</param>
/// <param name="robotGameObject">GameObject of the robot the manipulator will be attached to</param>
public bool InitializeManipulator(string directory)
{
ManipulatorObject = new GameObject("Manipulator");
ManipulatorObject.transform.position = robotStartPosition + manipulatorOffset;
RigidNode_Base.NODE_FACTORY = delegate(Guid guid) { return(new RigidNode(guid)); };
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
//TO-DO: Read .robot instead (from the new exporters if they are implemented). Maybe need a RobotSkeleton class
manipulatorNode = BXDExtensions.ReadSkeletonSafe(directory + Path.DirectorySeparatorChar + "skeleton") as RigidNode;
manipulatorNode.ListAllNodes(nodes);
//Load node_0 for attaching manipulator to robot
RigidNode node = (RigidNode)nodes[0];
node.CreateTransform(ManipulatorObject.transform);
if (!node.CreateMesh(directory + Path.DirectorySeparatorChar + node.ModelFileName))
{
Destroy(ManipulatorObject);
return(false);
}
node.CreateManipulatorJoint(gameObject);
node.MainObject.AddComponent <Tracker>().Trace = true;
//Load other nodes associated with the manipulator
for (int i = 1; i < nodes.Count; i++)
{
RigidNode otherNode = (RigidNode)nodes[i];
otherNode.CreateTransform(ManipulatorObject.transform);
if (!otherNode.CreateMesh(directory + Path.DirectorySeparatorChar + otherNode.ModelFileName))
{
Destroy(ManipulatorObject);
return(false);
}
otherNode.MainObject.AddComponent <Tracker>().Trace = true;
}
RootNode.GenerateWheelInfo();
for (int i = 1; i < nodes.Count; i++)
{
RigidNode otherNode = (RigidNode)nodes[i];
otherNode.CreateJoint(this);
}
RotateRobot(robotStartOrientation);
RobotHasManipulator = true;
return(true);
}
/// <summary>
/// Initializes physical robot based off of robot directory.
/// </summary>
/// <param name="directory">folder directory of robot</param>
/// <returns></returns>
public bool InitializeRobot(string directory, MainState source)
{
//Deletes all nodes if any exist
int childCount = transform.childCount;
for (int i = 0; i < childCount; ++i)
{
Destroy(transform.GetChild(i).gameObject);
}
mainState = source;
transform.position = robotStartPosition;
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new RigidNode(guid));
};
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
//Read .robot instead. Maybe need a RobotSkeleton class
rootNode = BXDJSkeleton.ReadSkeleton(directory + "\\skeleton.bxdj");
rootNode.ListAllNodes(nodes);
foreach (RigidNode_Base n in nodes)
{
RigidNode node = (RigidNode)n;
node.CreateTransform(transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName))
{
Debug.Log("Robot not loaded!");
return(false);
}
node.CreateJoint();
node.MainObject.AddComponent <Tracker>().Trace = true;
}
RotateRobot(robotStartOrientation);
RobotName = new DirectoryInfo(directory).Name;
isInitialized = true;
robotCamera = GameObject.Find("RobotCameraList").GetComponent <RobotCamera>();
//Attached to the main frame and face the front
robotCamera.AddCamera(transform.GetChild(0).transform);
//Attached to the first node and face the front
robotCamera.AddCamera(transform.GetChild(1).transform);
////Attached to main frame and face the back
robotCamera.AddCamera(transform.GetChild(0).transform, new Vector3(0, 0, 0), new Vector3(0, 180, 0));
return(true);
}
public bool LoadManipulator(string directory)
{
manipulatorObject = new GameObject("Manipulator");
//Set the manipulator transform to match with the position of node_0 of the robot. THIS ONE ACTUALLY DOES SOMETHING:
manipulatorObject.transform.position = GameObject.Find("Robot").transform.GetChild(0).transform.position;
//manipulatorObject.transform.position = robotStartPosition;
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new RigidNode(guid));
};
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
//TO-DO: Read .robot instead (from the new exporters if they are implemented). Maybe need a RobotSkeleton class
manipulatorNode = BXDJSkeleton.ReadSkeleton(directory + "\\skeleton.bxdj");
manipulatorNode.ListAllNodes(nodes);
//Load node_0 for attaching manipulator to robot
RigidNode node = (RigidNode)nodes[0];
node.CreateTransform(manipulatorObject.transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName))
{
Debug.Log("Robot not loaded!");
UnityEngine.Object.Destroy(manipulatorObject);
return(false);
}
node.CreateManipulatorJoint();
node.MainObject.AddComponent <Tracker>().Trace = true;
Tracker t = node.MainObject.GetComponent <Tracker>();
Debug.Log(t);
//Load other nodes associated with the manipulator
for (int i = 1; i < nodes.Count; i++)
{
RigidNode otherNode = (RigidNode)nodes[i];
otherNode.CreateTransform(manipulatorObject.transform);
if (!otherNode.CreateMesh(directory + "\\" + otherNode.ModelFileName))
{
Debug.Log("Robot not loaded!");
UnityEngine.Object.Destroy(manipulatorObject);
return(false);
}
otherNode.CreateJoint();
otherNode.MainObject.AddComponent <Tracker>().Trace = true;
t = otherNode.MainObject.GetComponent <Tracker>();
Debug.Log(t);
}
RotateRobot(robotStartOrientation);
return(true);
}
public static RigidNode_Base ExportSkeleton(List <ComponentOccurrence> occurrences)
{
if (occurrences.Count == 0)
{
throw new Exception("No components selected!");
}
SynthesisGUI.Instance.ExporterSetOverallText("Centering joints");
SynthesisGUI.Instance.ExporterReset();
SynthesisGUI.Instance.ExporterSetSubText("Centering 0 / 0");
SynthesisGUI.Instance.ExporterSetProgress(0);
SynthesisGUI.Instance.ExporterSetMeshes(2);
int numOccurrences = occurrences.Count;
SynthesisGUI.Instance.ExporterStepOverall();
SynthesisGUI.Instance.ExporterSetOverallText("Getting rigid info");
Console.WriteLine("Get rigid info...");
//Group components into rigid bodies.
NameValueMap options = InventorManager.Instance.TransientObjects.CreateNameValueMap();
options.Add("DoubleBearing", false);
RigidBodyResults rigidResults = InventorManager.Instance.AssemblyDocument.ComponentDefinition.RigidBodyAnalysis(options);
Console.WriteLine("Got rigid info...");
CustomRigidResults customRigid = new CustomRigidResults(rigidResults);
Console.WriteLine("Build model...");
RigidBodyCleaner.CleanGroundedBodies(customRigid);
//After this point, all grounded groups have been merged into one CustomRigidGroup, and their joints have been updated.
RigidNode baseNode = RigidBodyCleaner.BuildAndCleanDijkstra(customRigid);
Console.WriteLine("Built");
Console.WriteLine(baseNode.ToString());
SynthesisGUI.Instance.ExporterStepOverall();
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
baseNode.ListAllNodes(nodes);
foreach (RigidNode_Base node in nodes)
{
node.ModelFileName = ((RigidNode)node).group.ToString();
node.ModelFullID = node.GetModelID();
}
return(baseNode);
}
public static float GetLinearPositionRelativeToParent(RigidNode baseNode)
{
RigidBody baseRB = (RigidBody)baseNode.MainObject.GetComponent <BRigidBody>().GetCollisionObject();
Vector3 baseDirection = BulletSharp.Math.Quaternion.RotationMatrix(baseRB.WorldTransform).ToUnity() * baseNode.GetJoint <BTypedConstraint>().localConstraintAxisX;
baseDirection.Normalize();
RigidBody parentRB = (RigidBody)((RigidNode)baseNode.GetParent()).MainObject.GetComponent <BRigidBody>().GetCollisionObject();
Vector3 difference = baseRB.WorldTransform.Origin.ToUnity() - parentRB.WorldTransform.Origin.ToUnity();
return(-Vector3.Dot(baseDirection, difference));
}
/// <summary>
/// Updates all emulation sensor values.
/// </summary>
/// <param name="emuList"></param>
private static void UpdateEmulationSensors(List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList)
{
int iter = 0;
foreach (Synthesis.Robot.RobotBase.EmuNetworkInfo a in emuList)
{
RigidNode rigidNode = null;
try
{
rigidNode = (RigidNode)(a.wheel);
}
catch (Exception e)
{
Debug.Log(e.StackTrace);
}
BRaycastWheel bRaycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
if (a.RobotSensor.type == RobotSensorType.ENCODER)
{
//BRaycastRobot robot = rigidNode.MainObject.GetComponent<BRaycastRobot>();
double wheelRadius = 3 / 39.3701;// robot.RaycastRobot.GetWheelInfo(0).WheelsRadius;
Vector3 currentPos = bRaycastWheel.transform.position;
//double displacement = (Math.Sqrt(currentPos.x * currentPos.x + currentPos.z * currentPos.z) - (Math.Sqrt(a.previousPosition.x*a.previousPosition.x + a.previousPosition.z * a.previousPosition.z)));
double displacement = ((currentPos - a.previousPosition).magnitude) * Math.Sign(bRaycastWheel.GetWheelSpeed());
double angleDisplacement = (displacement) / (2 * 3.1415 * wheelRadius);
a.encoderTickCount += angleDisplacement * a.RobotSensor.conversionFactor;
a.previousPosition = currentPos;
var portAType = a.RobotSensor.conTypePortA.ToString() == "DIO" ? "DI" : a.RobotSensor.conTypePortA.ToString();
var portBType = a.RobotSensor.conTypePortB.ToString() == "DIO" ? "DI" : a.RobotSensor.conTypePortB.ToString();
if (InputManager.Instance.Roborio.Encoders[iter] == null)
{
InputManager.Instance.Roborio.Encoders[iter] = new EncoderData();
}
if (Synthesis.GUI.EmulationDriverStation.Instance.isRunCode)
{
InputManager.Instance.Roborio.Encoders[iter].updateEncoder((int)a.RobotSensor.portA, portAType, (int)a.RobotSensor.portB, portBType, (int)a.encoderTickCount);
}
iter++;
}
}
}
public static float GetAngleBetweenChildAndParent(RigidNode child)
{
BHingedConstraint hinge = child.GetJoint <BHingedConstraint>();
if (hinge != null)
{
return(((HingeConstraint)hinge.GetConstraint()).HingeAngle);
}
RigidBody childRB = (RigidBody)child.MainObject.GetComponent <BRigidBody>().GetCollisionObject();
RigidBody parentRB = (RigidBody)((RigidNode)child.GetParent()).MainObject.GetComponent <BRigidBody>().GetCollisionObject();
Vector3 childUp = BulletSharp.Math.Quaternion.RotationMatrix(childRB.WorldTransform).ToUnity() * Vector3.up;
Vector3 parentUp = BulletSharp.Math.Quaternion.RotationMatrix(parentRB.WorldTransform).ToUnity() * Vector3.up;
return(MathfExt.ToDegrees(Mathf.Acos(Vector3.Dot(childUp, parentUp) / (childUp.magnitude * parentUp.magnitude))));
}
/// <summary>
/// Updates the physics of the robot.
/// </summary>
protected override void UpdatePhysics()
{
var begin = DateTime.Now;
base.UpdatePhysics();
if (!state.IsMetric)
{
Speed = (float)Math.Round(Speed * 3.28084, 3);
Acceleration = (float)Math.Round(Acceleration * 3.28084, 3);
Weight = (float)Math.Round(Weight * 2.20462, 3);
}
if (GameObject.Find("Field") != null)
{
if (gameObject.transform.GetChild(0).position.y < GameObject.Find("Field").transform.position.y - 2)
{
if (robotStartPosition.y < GameObject.Find("Field").transform.position.y)
{
robotStartPosition.y = GameObject.Find("Field").transform.position.y + 1.25f;
}
BeginReset();
EndReset();
}
}
#region Encoder Calculations
foreach (EmuNetworkInfo a in emuList)
{
RigidNode rigidNode = null;
try
{
rigidNode = (RigidNode)(a.wheel);
}
catch (Exception e)
{
UnityEngine.Debug.Log(e.StackTrace);
}
BRaycastWheel bRaycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
}
#endregion
}
/// <summary>
/// The lightweight equivalent of the 'Add From Inventor' button in the <see cref="ExporterForm"/>. Used in <see cref="ExportMeshesLite(RigidNode_Base)"/>
/// </summary>
/// <param name="occurrences"></param>
/// <returns></returns>
public RigidNode_Base ExportSkeleton(List <ComponentOccurrence> occurrences)
{
if (occurrences.Count == 0)
{
throw new Exporter.EmptyAssemblyException();
}
#region Build Models
//Getting Rigid Body Info...
SetProgress("Getting physics info...", occurrences.Count, occurrences.Count + 3);
NameValueMap RigidGetOptions = InventorManager.Instance.TransientObjects.CreateNameValueMap();
RigidGetOptions.Add("DoubleBearing", false);
RigidBodyResults RawRigidResults = InventorManager.Instance.AssemblyDocument.ComponentDefinition.RigidBodyAnalysis(RigidGetOptions);
//Getting Rigid Body Info...Done
CustomRigidResults RigidResults = new CustomRigidResults(RawRigidResults);
//Building Model...
SetProgress("Building model...", occurrences.Count + 1, occurrences.Count + 3);
RigidBodyCleaner.CleanGroundedBodies(RigidResults);
RigidNode baseNode = RigidBodyCleaner.BuildAndCleanDijkstra(RigidResults);
//Building Model...Done
#endregion
#region Cleaning Up
//Cleaning Up...
SetProgress("Cleaning up...", occurrences.Count + 2, occurrences.Count + 3);
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
baseNode.ListAllNodes(nodes);
foreach (RigidNode_Base node in nodes)
{
node.ModelFileName = ((RigidNode)node).group.ToString();
node.ModelFullID = node.GetModelID();
}
//Cleaning Up...Done
#endregion
SetProgress("Done", occurrences.Count + 3, occurrences.Count + 3);
return(baseNode);
}
/// <summary>
/// Generates the robot from the list of <see cref="RigidNode_Base"/>s and the
/// number of wheels, and updates the collective mass.
/// </summary>
/// <param name="nodes"></param>
/// <param name="numWheels"></param>
/// <param name="collectiveMass"></param>
/// <returns></returns>
protected override bool ConstructRobot(List <RigidNode_Base> nodes, ref float collectiveMass)
{
if (!base.ConstructRobot(nodes, ref collectiveMass))
{
return(false);
}
foreach (RigidNode_Base n in nodes)
{
RigidNode node = (RigidNode)n;
if (node.MainObject.GetComponent <BRigidBody>() != null)
{
node.MainObject.AddComponent <Tracker>().Trace = true;
}
}
return(true);
}
public void AddRigidNode(RigidNode rigid)
{
RootRigid.ChildList.Add(rigid);
}
/// <summary>
/// Initializes physical robot based off of robot directory.
/// </summary>
/// <param name="directory">folder directory of robot</param>
/// <returns></returns>
public bool InitializeRobot(string directory, MainState source)
{
#region Robot Initialization
RobotDirectory = directory;
//Deletes all nodes if any exist, take the old node transforms out from the robot object
int childCount = transform.childCount;
for (int i = childCount - 1; i >= 0; i--)
{
Transform child = transform.GetChild(i);
//If this isn't done, the game object is destroyed but the parent-child transform relationship remains!
child.parent = null;
Destroy(child.gameObject);
}
//Detach and destroy all sensors on the original robot
SensorManager sensorManager = GameObject.Find("SensorManager").GetComponent <SensorManager>();
sensorManager.ResetSensorLists();
//Removes Driver Practice component if it exists
if (dpmRobot != null)
{
Destroy(dpmRobot);
}
mainState = source; //stores the main state object
transform.position = robotStartPosition; //Sets the position of the object to the set spawn point
if (!File.Exists(directory + "\\skeleton.bxdj"))
{
return(false);
}
//Loads the node and skeleton data
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new RigidNode(guid));
};
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
rootNode = BXDJSkeleton.ReadSkeleton(directory + "\\skeleton.bxdj");
rootNode.ListAllNodes(nodes);
//Initializes the wheel variables
int numWheels = nodes.Count(x => x.HasDriverMeta <WheelDriverMeta>() && x.GetDriverMeta <WheelDriverMeta>().type != WheelType.NOT_A_WHEEL);
float collectiveMass = 0f;
int isMixAndMatch = PlayerPrefs.GetInt("mixAndMatch");
if (isMixAndMatch == 1 && !MixAndMatchMode.isMecanum)
{
//Load Node_0
RigidNode node = (RigidNode)nodes[0];
node.CreateTransform(transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName))
{
Debug.Log("Robot not loaded!");
return(false);
}
node.CreateJoint(numWheels);
if (node.PhysicalProperties != null)
{
collectiveMass += node.PhysicalProperties.mass;
}
if (node.MainObject.GetComponent <BRigidBody>() != null)
{
node.MainObject.AddComponent <Tracker>().Trace = true;
}
//Load the other nodes (wheels)
string wheelDirectory = PlayerPrefs.GetString("simSelectedWheel");
BXDAMesh mesh = new BXDAMesh();
mesh.ReadFromFile(wheelDirectory + "\\node_0.bxda");
List <Mesh> meshList = new List <Mesh>();
List <Material[]> materialList = new List <Material[]>();
RigidNode wheelNode = (RigidNode)BXDJSkeleton.ReadSkeleton(wheelDirectory + "\\skeleton.bxdj");
Material[] materials = new Material[0];
AuxFunctions.ReadMeshSet(mesh.meshes, delegate(int id, BXDAMesh.BXDASubMesh sub, Mesh meshu)
{
meshList.Add(meshu);
materials = new Material[meshu.subMeshCount];
for (int i = 0; i < materials.Length; i++)
{
materials[i] = sub.surfaces[i].AsMaterial(true);
}
materialList.Add(materials);
//meshObject.GetComponent<MeshRenderer>().materials = materials;
}, true);
for (int i = 1; i < nodes.Count; i++)
{
node = (RigidNode)nodes[i];
node.CreateTransform(transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName))
{
Debug.Log("Robot not loaded!");
return(false);
}
if (node.HasDriverMeta <WheelDriverMeta>())
{
int chldCount = node.MainObject.transform.childCount;
for (int j = 0; j < chldCount; j++)
{
Destroy(node.MainObject.transform.GetChild(j).gameObject);
}
int k = 0;
foreach (Mesh meshObject in meshList)
{
Debug.Log("Mesh Object" + meshObject);
GameObject meshObj = new GameObject(node.MainObject.name + "_mesh");
meshObj.transform.parent = node.MainObject.transform;
meshObj.AddComponent <MeshFilter>().mesh = meshObject;
meshObj.transform.localPosition = -meshObject.bounds.center;
//Take out this line if you want some snazzy pink wheels
meshObj.AddComponent <MeshRenderer>().materials = materialList[k];
k++;
}
node.MainObject.GetComponentInChildren <MeshRenderer>().materials = materials;
}
//node.MainObject.transform.GetChild(0).localPosition = -node.MainObject.GetComponentInChildren<MeshFilter>().mesh.bounds.center;// -node.MainObject.transform.localPosition;
//Bounds b = node.MainObject.GetComponentInChildren<MeshFilter>().mesh.bounds;
// Debug.Log(b.center);
//b.center = node.MainObject.transform.position;
//node.MainObject.GetComponentInChildren<MeshFilter>().mesh.bounds = b;
node.CreateJoint(numWheels);
if (node.HasDriverMeta <WheelDriverMeta>())
{
float radius = PlayerPrefs.GetFloat("wheelRadius");
node.MainObject.GetComponent <BRaycastWheel>().Radius = radius;
}
if (node.PhysicalProperties != null)
{
collectiveMass += node.PhysicalProperties.mass;
}
if (node.MainObject.GetComponent <BRigidBody>() != null)
{
node.MainObject.AddComponent <Tracker>().Trace = true;
}
}
}
else
{
//Initializes the nodes
foreach (RigidNode_Base n in nodes)
{
RigidNode node = (RigidNode)n;
node.CreateTransform(transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName))
{
Debug.Log("Robot not loaded!");
return(false);
}
node.CreateJoint(numWheels);
if (node.PhysicalProperties != null)
{
collectiveMass += node.PhysicalProperties.mass;
}
if (node.MainObject.GetComponent <BRigidBody>() != null)
{
node.MainObject.AddComponent <Tracker>().Trace = true;
}
}
}
#endregion
//Get the offset from the first node to the robot for new robot start position calculation
//This line is CRITICAL to new reset position accuracy! DON'T DELETE IT!
nodeToRobotOffset = gameObject.transform.GetChild(0).localPosition - robotStartPosition;
foreach (BRaycastRobot r in GetComponentsInChildren <BRaycastRobot>())
{
r.RaycastRobot.OverrideMass = collectiveMass;
r.RaycastRobot.RootRigidBody = (RigidBody)((RigidNode)nodes[0]).MainObject.GetComponent <BRigidBody>().GetCollisionObject();
}
RotateRobot(robotStartOrientation);
RobotName = new DirectoryInfo(directory).Name;
isInitialized = true;
//Initializes Driver Practice component
dpmRobot = gameObject.AddComponent <DriverPracticeRobot>();
dpmRobot.Initialize(directory);
//Initializing robot cameras
bool hasRobotCamera = false;
//If you are getting an error referencing this line, it is likely that the Game Object "RobotCameraList" in Scene.unity does not have the RobotCameraManager script attached to it.
robotCameraManager = GameObject.Find("RobotCameraList").GetComponent <RobotCameraManager>();
//Loop through robotCameraList and check if any existing camera should attach to this robot
foreach (GameObject robotCamera in robotCameraManager.GetRobotCameraList())
{
if (robotCamera.GetComponent <RobotCamera>().robot.Equals(this))
{
//Recover the robot camera configurations
robotCamera.GetComponent <RobotCamera>().RecoverConfiguration();
hasRobotCamera = true;
}
}
//Add new cameras to the robot if there is none robot camera belong to the current robot (which means it is a new robot)
if (!hasRobotCamera)
{
//Attached to the main frame and face the front
robotCameraManager.AddCamera(this, transform.GetChild(0).transform);
//Attached to the first node and face the front
if (transform.childCount > 1)
{
robotCameraManager.AddCamera(this, transform.GetChild(1).transform);
}
////Attached to main frame and face the back
robotCameraManager.AddCamera(this, transform.GetChild(0).transform, new Vector3(0, 0, 0), new Vector3(0, 180, 0));
}
return(true);
}
/// <summary>
/// Initializes physical robot based off of robot directory.
/// </summary>
/// <param name="directory">folder directory of robot</param>
/// <returns></returns>
public bool InitializeRobot(string directory)
{
RobotDirectory = directory;
RobotName = new DirectoryInfo(directory).Name;
//Deletes all nodes if any exist, take the old node transforms out from the robot object
foreach (Transform child in transform)
{
Destroy(child.gameObject);
}
robotStartPosition = FieldDataHandler.robotSpawn != new Vector3(99999, 99999, 99999) ? FieldDataHandler.robotSpawn : robotStartPosition;
transform.position = robotStartPosition; //Sets the position of the object to the set spawn point
if (!File.Exists(directory + "\\skeleton.bxdj"))
{
return(false);
}
OnInitializeRobot();
//Loads the node and skeleton data
RigidNode_Base.NODE_FACTORY = delegate(Guid guid) { return(new RigidNode(guid)); };
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
RootNode = BXDJSkeleton.ReadSkeleton(directory + "\\skeleton.bxdj") as RigidNode;
RootNode.ListAllNodes(nodes);
Debug.Log(RootNode.driveTrainType.ToString());
emuList = new List <EmuNetworkInfo>();
foreach (RigidNode_Base Base in RootNode.ListAllNodes())
{
try
{
if (Base.GetSkeletalJoint().attachedSensors != null)
{
foreach (RobotSensor sensor in Base.GetSkeletalJoint().attachedSensors)
{
if (sensor.type == RobotSensorType.ENCODER)
{
EmuNetworkInfo emuStruct = new EmuNetworkInfo();
emuStruct.encoderTickCount = 0;
emuStruct.RobotSensor = sensor;
emuStruct.wheel = Base;
emuStruct.wheel_radius = 0;
emuList.Add(emuStruct);
}
}
}
}
catch (Exception e)
{
Debug.Log(e.ToString());
}
}
//Initializes the wheel variables
float collectiveMass = 0f;
if (!ConstructRobot(nodes, ref collectiveMass))
{
return(false);
}
foreach (BRaycastRobot r in GetComponentsInChildren <BRaycastRobot>())
{
r.RaycastRobot.OverrideMass = collectiveMass;
r.RaycastRobot.RootRigidBody = (RigidBody)((RigidNode)nodes[0]).MainObject.GetComponent <BRigidBody>().GetCollisionObject();
}
OnRobotSetup();
RotateRobot(robotStartOrientation);
return(true);
}
/// <summary>
/// Updates the motors on the manipulator in mix and match mode. Called every frame.
/// </summary>
/// <param name="skeleton"></param>
/// <param name="dioModules"></param>
/// <param name="controlIndex"></param>
public static void UpdateManipulatorMotors(RigidNode_Base skeleton, int controlIndex, List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList)
{
UpdateAllOutputs(controlIndex, emuList);
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
if (raycastWheel != null)
{
float force = GetOutput(rigidNode.GetSkeletalJoint().cDriver);
if (rigidNode.GetSkeletalJoint().cDriver.InputGear != 0 && rigidNode.GetSkeletalJoint().cDriver.OutputGear != 0)
{
force *= Convert.ToSingle(rigidNode.GetSkeletalJoint().cDriver.InputGear / rigidNode.GetSkeletalJoint().cDriver.OutputGear);
}
raycastWheel.ApplyForce(force);
}
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null)
{
float output = GetOutput(rigidNode.GetSkeletalJoint().cDriver);
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
if (rigidNode.GetSkeletalJoint().cDriver.InputGear != 0 && rigidNode.GetSkeletalJoint().cDriver.OutputGear != 0)
{
impulse *= Convert.ToSingle(rigidNode.GetSkeletalJoint().cDriver.InputGear / rigidNode.GetSkeletalJoint().cDriver.OutputGear);
}
if (rigidNode.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WheelMaxSpeed;
impulse = WheelMotorImpulse;
friction = WheelCoastFriction;
}
else
{
maxSpeed = HingeMaxSpeed;
impulse = HingeMotorImpulse;
friction = HingeCostFriction;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = output > 0f ? maxSpeed : output < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = rigidNode.GetSkeletalJoint().cDriver.hasBrake ? HingeMotorImpulse : output == 0f ? friction : Mathf.Abs(output * impulse);
}
else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
if (rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
float output = GetOutput(rigidNode.GetSkeletalJoint().cDriver);
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MaxSliderForce;
sc.TargetLinearMotorVelocity = output * MaxSliderSpeed;
}
}
}
}
}
public static void UpdateAllMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex, bool mecanum)
{
bool IsMecanum = mecanum;
int reverse = -1;
float[] pwm = new float[10];
float[] can = new float[10];
if (dioModules[0] != null)
{
pwm = dioModules[0].pwmValues;
can = dioModules[0].canValues;
}
if (IsMecanum)
{
pwm[(int)MecanumPorts.FRONT_RIGHT] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? reverse * SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? reverse * -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? reverse * -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? reverse * SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.O) ? reverse * SPEED_ARROW_PWM : 0.0f) + //Left Rotate
(Input.GetKey(KeyCode.P) ? reverse * -SPEED_ARROW_PWM : 0.0f); //Right Rotate
pwm[(int)MecanumPorts.BACK_LEFT] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.O) ? -SPEED_ARROW_PWM : 0.0f) + //Left Rotate
(Input.GetKey(KeyCode.P) ? SPEED_ARROW_PWM : 0.0f); //Right Rotate
pwm[(int)MecanumPorts.FRONT_LEFT] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? -SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.O) ? -SPEED_ARROW_PWM : 0.0f) + //Left Rotate
(Input.GetKey(KeyCode.P) ? SPEED_ARROW_PWM : 0.0f); //Right Rotate
pwm[(int)MecanumPorts.BACK_RIGHT] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? reverse * SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? reverse * -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? reverse * SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? reverse * -SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.O) ? reverse * SPEED_ARROW_PWM : 0.0f) + //Left Rotate
(Input.GetKey(KeyCode.P) ? reverse * -SPEED_ARROW_PWM : 0.0f); //Right Rotate
}
else
{
pwm[0] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f);
pwm[1] +=
(InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) +
(InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f);
pwm[2] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm2Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm2Neg)) ? -SPEED_ARROW_PWM : 0f;
pwm[3] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm3Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm3Neg)) ? -SPEED_ARROW_PWM : 0f;
pwm[4] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm4Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm4Neg)) ? -SPEED_ARROW_PWM : 0f;
pwm[5] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm5Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm5Neg)) ? -SPEED_ARROW_PWM : 0f;
pwm[6] +=
(InputControl.GetButton(Controls.buttons[controlIndex].pwm6Plus)) ? SPEED_ARROW_PWM :
(InputControl.GetButton(Controls.buttons[controlIndex].pwm6Neg)) ? -SPEED_ARROW_PWM : 0f;
}
List <RigidNode_Base> listOfSubNodes = new List <RigidNode_Base>();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
if (rigidNode.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WHEEL_MAX_SPEED;
impulse = WHEEL_MOTOR_IMPULSE;
friction = WHEEL_COAST_FRICTION;
}
else
{
maxSpeed = HINGE_MAX_SPEED;
impulse = HINGE_MOTOR_IMPULSE;
friction = HINGE_COAST_FRICTION;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MAX_SLIDER_FORCE;
sc.TargetLinearMotorVelocity = pwm[i] * MAX_SLIDER_SPEED;
}
}
}
}
}
}
/// <summary>
/// Updates the motors on the manipulator in mix and match mode. Called every frame.
/// </summary>
/// <param name="skeleton"></param>
/// <param name="dioModules"></param>
/// <param name="controlIndex"></param>
public static void UpdateManipulatorMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex)
{
float[] pwm;
float[] can;
if (dioModules[0] != null)
{
pwm = dioModules[0].pwmValues;
can = dioModules[0].canValues;
}
else
{
pwm = new float[10];
can = new float[10];
}
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SpeedArrowPwm);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SpeedArrowPwm);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SpeedArrowPwm);
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
if (raycastWheel != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.port1 == i + 1)
{
float force = pwm[i];
if (rigidNode.GetSkeletalJoint().cDriver.InputGear != 0 && rigidNode.GetSkeletalJoint().cDriver.OutputGear != 0)
{
force *= Convert.ToSingle(rigidNode.GetSkeletalJoint().cDriver.InputGear / rigidNode.GetSkeletalJoint().cDriver.OutputGear);
}
raycastWheel.ApplyForce(force);
}
}
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.port1 == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
if (rigidNode.GetSkeletalJoint().cDriver.InputGear != 0 && rigidNode.GetSkeletalJoint().cDriver.OutputGear != 0)
{
impulse *= Convert.ToSingle(rigidNode.GetSkeletalJoint().cDriver.InputGear / rigidNode.GetSkeletalJoint().cDriver.OutputGear);
}
if (rigidNode.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WheelMaxSpeed;
impulse = WheelMotorImpulse;
friction = WheelCoastFriction;
}
else
{
maxSpeed = HingeMaxSpeed;
impulse = HingeMotorImpulse;
friction = HingeCostFriction;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = rigidNode.GetSkeletalJoint().cDriver.hasBrake ? HingeMotorImpulse : pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
if (rigidNode.GetSkeletalJoint().cDriver.port1 == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MaxSliderForce;
sc.TargetLinearMotorVelocity = pwm[i] * MaxSliderSpeed;
}
}
}
}
}
}
/// <summary>
/// Updates the motors on the manipulator in mix and match mode. Called every frame.
/// </summary>
/// <param name="skeleton"></param>
/// <param name="dioModules"></param>
/// <param name="controlIndex"></param>
public static void UpdateManipulatorMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex)
{
float[] pwm;
float[] can;
if (dioModules[0] != null)
{
pwm = dioModules[0].pwmValues;
can = dioModules[0].canValues;
}
else
{
pwm = new float[10];
can = new float[10];
}
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SpeedArrowPwm);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SpeedArrowPwm);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SpeedArrowPwm);
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
SkeletalJoint_Base joint = rigidNode.GetSkeletalJoint();
if (raycastWheel != null)
{
if (joint.cDriver.port1 == i + 1)
{
float output = pwm[i];
MotorType motorType = joint.cDriver.GetMotorType();
float torque = motorType == MotorType.GENERIC ? 2.42f : 60 * motorDefinition[motorType].baseTorque - motorDefinition[motorType].slope * raycastWheel.GetWheelSpeed() / 9.549297f;
if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0)
{
torque /= Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear);
}
raycastWheel.ApplyForce(output, torque, motorType == MotorType.GENERIC);
}
}
if (joint != null && joint.cDriver != null)
{
if (joint.cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null)
{
if (joint.cDriver.port1 == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
friction = HingeCostFriction;
MotorType motorType = joint.cDriver.GetMotorType();
Motor motor = motorType == MotorType.GENERIC ? new Motor(10f, 4f) : motorDefinition[motorType];
maxSpeed = motor.maxSpeed;
impulse = motor.baseTorque - motor.slope * ((RigidBody)(rigidNode.MainObject.GetComponent <BRigidBody>().GetCollisionObject())).AngularVelocity.Length / 9.549297f;
if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0)
{
float gearRatio = Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear);
impulse /= gearRatio;
maxSpeed *= gearRatio;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = joint.cDriver.hasBrake ? motor.baseTorque : pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (joint.cDriver.GetDriveType().IsElevator())
{
if (joint.cDriver.port1 == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MaxSliderForce;
sc.TargetLinearMotorVelocity = pwm[i] * MaxSliderSpeed;
}
}
else if (joint.cDriver.GetDriveType().IsPneumatic() && rigidNode.HasDriverMeta <PneumaticDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
float output = motors[joint.cDriver.port1 - 1];
float psi = node.GetDriverMeta <PneumaticDriverMeta>().pressurePSI * 6894.76f;
float width = node.GetDriverMeta <PneumaticDriverMeta>().widthMM * 0.001f;
float stroke = (sc.UpperLinearLimit - sc.LowerLinearLimit) / 0.01f;
float force = psi * ((float)Math.PI) * width * width / 4f;
float speed = stroke / 60f;
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = force;
sc.TargetLinearMotorVelocity = sc.TargetLinearMotorVelocity != 0 && output == 0 ? sc.TargetLinearMotorVelocity : output * speed;
}
}
}
}
}
public static void UpdateAllMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules)
{
float[] pwm = dioModules[0].pwmValues;
float[] can = dioModules[0].canValues;
if (Input.anyKey)
{
pwm[0] +=
(Input.GetKey(KeyCode.UpArrow) ? SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.DownArrow) ? -SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.LeftArrow) ? SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.RightArrow) ? -SPEED_ARROW_PWM : 0.0f);
pwm[1] +=
(Input.GetKey(KeyCode.UpArrow) ? -SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.DownArrow) ? SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.LeftArrow) ? SPEED_ARROW_PWM : 0.0f) +
(Input.GetKey(KeyCode.RightArrow) ? -SPEED_ARROW_PWM : 0.0f);
pwm[2] += Input.GetKey(KeyCode.Alpha1) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha2) ? -SPEED_ARROW_PWM : 0f;
pwm[3] += Input.GetKey(KeyCode.Alpha3) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha4) ? -SPEED_ARROW_PWM : 0f;
pwm[4] += Input.GetKey(KeyCode.Alpha5) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha6) ? -SPEED_ARROW_PWM : 0f;
pwm[5] += Input.GetKey(KeyCode.Alpha7) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha8) ? -SPEED_ARROW_PWM : 0f;
pwm[6] += Input.GetKey(KeyCode.Alpha9) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha0) ? -SPEED_ARROW_PWM : 0f;
}
List <RigidNode_Base> listOfSubNodes = new List <RigidNode_Base>();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
if (rigidNode.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WHEEL_MAX_SPEED;
impulse = WHEEL_MOTOR_IMPULSE;
friction = WHEEL_COAST_FRICTION;
}
else
{
maxSpeed = HINGE_MAX_SPEED;
impulse = HINGE_MOTOR_IMPULSE;
friction = HINGE_COAST_FRICTION;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MAX_SLIDER_FORCE;
sc.TargetLinearMotorVelocity = pwm[i] * MAX_SLIDER_SPEED;
}
}
}
}
}
}
/// <summary>
/// Generates the robot from the list of <see cref="RigidNode_Base"/>s and the
/// number of wheels, and updates the collective mass.
/// </summary>
/// <param name="nodes"></param>
/// <param name="numWheels"></param>
/// <param name="collectiveMass"></param>
/// <returns></returns>
protected override bool ConstructRobot(List <RigidNode_Base> nodes, ref float collectiveMass)
{
if (IsMecanum())
{
return(base.ConstructRobot(nodes, ref collectiveMass));
}
//Load Node_0, the base of the robot
RigidNode node = (RigidNode)nodes[0];
node.CreateTransform(transform);
if (!node.CreateMesh(RobotDirectory + "\\" + node.ModelFileName, true, wheelMass))
{
return(false);
}
node.CreateJoint(this);
if (node.PhysicalProperties != null)
{
collectiveMass += node.PhysicalProperties.mass;
}
if (node.MainObject.GetComponent <BRigidBody>() != null)
{
node.MainObject.AddComponent <Tracker>().Trace = true;
}
//Get the wheel mesh data from the file they are stored in. They are stored as .bxda files. This may need to update if exporters/file types change.
BXDAMesh mesh = new BXDAMesh();
mesh.ReadFromFile(wheelPath + "\\node_0.bxda");
List <Mesh> meshList = new List <Mesh>();
List <Material[]> materialList = new List <Material[]>();
RigidNode wheelNode = (RigidNode)BXDJSkeleton.ReadSkeleton(wheelPath + "\\skeleton.bxdj");
Material[] materials = new Material[0];
Auxiliary.ReadMeshSet(mesh.meshes, delegate(int id, BXDAMesh.BXDASubMesh sub, Mesh meshu)
{
meshList.Add(meshu);
materials = new Material[meshu.subMeshCount];
for (int i = 0; i < materials.Length; i++)
{
materials[i] = sub.surfaces[i].AsMaterial(true);
}
materialList.Add(materials);
}, true);
//Loads the other nodes from the original robot
for (int i = 1; i < nodes.Count; i++)
{
node = (RigidNode)nodes[i];
node.CreateTransform(transform);
if (!node.CreateMesh(RobotDirectory + "\\" + node.ModelFileName, true, wheelMass))
{
return(false);
}
//If the node is a wheel, destroy the original wheel mesh and replace it with the wheels selected in MaM
if (node.HasDriverMeta <WheelDriverMeta>())
{
int chldCount = node.MainObject.transform.childCount;
for (int j = 0; j < chldCount; j++)
{
Destroy(node.MainObject.transform.GetChild(j).gameObject);
}
int k = 0;
Vector3?offset = null;
foreach (Mesh meshObject in meshList)
{
GameObject meshObj = new GameObject(node.MainObject.name + "_mesh");
meshObj.transform.parent = node.MainObject.transform;
meshObj.AddComponent <MeshFilter>().mesh = meshObject;
if (!offset.HasValue)
{
offset = meshObject.bounds.center;
}
meshObj.transform.localPosition = -offset.Value;
//Take out this line if you want some snazzy pink wheels
meshObj.AddComponent <MeshRenderer>().materials = materialList[k];
k++;
}
node.MainObject.GetComponentInChildren <MeshRenderer>().materials = materials;
}
}
RootNode.GenerateWheelInfo();
//Create the joints that interact with physics
for (int i = 1; i < nodes.Count; i++)
{
node = (RigidNode)nodes[i];
node.CreateJoint(this, wheelFriction, wheelLateralFriction);
if (node.HasDriverMeta <WheelDriverMeta>())
{
node.MainObject.GetComponent <BRaycastWheel>().Radius = wheelRadius;
}
if (node.PhysicalProperties != null)
{
collectiveMass += node.PhysicalProperties.mass;
}
if (node.MainObject.GetComponent <BRigidBody>() != null)
{
node.MainObject.AddComponent <Tracker>().Trace = true;
}
}
return(true);
}
/// <summary>
/// Merges any groups that are connected only with constraints and generate a node tree.
/// </summary>
/// <remarks>
/// This starts at whichever rigid group is grounded, then branches out along rigid joints from there.
/// If the rigid joint is movable (made of assembly joint(s)) then another node is created, if the joint
/// is constraint-only then the leaf node is merged into the current branch.
/// </remarks>
/// <param name="results">Rigid results to clean</param>
public static RigidNode BuildAndCleanDijkstra(CustomRigidResults results)
{
Dictionary <CustomRigidGroup, HashSet <CustomRigidGroup> > constraints = new Dictionary <CustomRigidGroup, HashSet <CustomRigidGroup> >();
Dictionary <CustomRigidGroup, HashSet <CustomRigidGroup> > joints = new Dictionary <CustomRigidGroup, HashSet <CustomRigidGroup> >();
GenerateJointMaps(results, joints, constraints);
// Mapping rigid group to merge-into group
Dictionary <CustomRigidGroup, CustomRigidGroup> mergePattern = new Dictionary <CustomRigidGroup, CustomRigidGroup>();
// Mapping rigid group to skeletal node
Dictionary <CustomRigidGroup, RigidNode> baseNodes = new Dictionary <CustomRigidGroup, RigidNode>();
// Deffered joint creation. Required so merge can take place.
List <PlannedJoint> plannedJoints = new List <PlannedJoint>();
// The base of the skeletal tree
RigidNode baseRoot = null;
// All the currently open groups as an array {currentGroup, mergeIntoGroup}
List <CustomRigidGroup[]> openNodes = new List <CustomRigidGroup[]>();
// All the groups that have been processed. (Closed nodes)
HashSet <CustomRigidGroup> closedNodes = new HashSet <CustomRigidGroup>();
// Find the first grounded group, the start point for dijkstra's algorithm.
foreach (CustomRigidGroup grp in results.groups)
{
if (grp.grounded)
{
openNodes.Add(new CustomRigidGroup[] { grp, grp });
closedNodes.Add(grp);
baseNodes.Add(grp, baseRoot = new RigidNode(Guid.NewGuid(), grp));
break; //Should only contain one grounded group, as they have all been merged together.
}
}
Console.WriteLine("Determining merge commands");
while (openNodes.Count > 0)
{
List <CustomRigidGroup[]> newOpen = new List <CustomRigidGroup[]>();
foreach (CustomRigidGroup[] node in openNodes)
{
// Get all connections
HashSet <CustomRigidGroup> cons = constraints[node[0]];
HashSet <CustomRigidGroup> jons = joints[node[0]];
foreach (CustomRigidGroup jonConn in jons)
{
if (!closedNodes.Add(jonConn)) //Moves on to next if the connected component is already in closedNodes.
{
continue;
}
RigidNode rnode = new RigidNode(Guid.NewGuid(), jonConn); //Makes a new rigid node for the connected component.
baseNodes.Add(jonConn, rnode);
//Find the actual joint between the two components.
foreach (CustomRigidJoint jnt in results.joints)
{
if (jnt.joints.Count > 0 && ((jnt.groupOne.Equals(jonConn) && jnt.groupTwo.Equals(node[0])) ||
(jnt.groupOne.Equals(node[0]) && jnt.groupTwo.Equals(jonConn))))
{
PlannedJoint pJoint = new PlannedJoint();
pJoint.joint = jnt;
pJoint.parentNode = baseNodes[node[1]];
pJoint.node = rnode;
plannedJoints.Add(pJoint);
newOpen.Add(new CustomRigidGroup[] { jonConn, jonConn });
break;
}
}
}
foreach (CustomRigidGroup consConn in cons)
{
if (!closedNodes.Add(consConn))
{
continue;
}
mergePattern.Add(consConn, node[1]);
newOpen.Add(new CustomRigidGroup[] { consConn, node[1] }); //Uses node[1] to ensure all constrained groups are merged into the same group.
}
}
openNodes = newOpen;
}
Console.WriteLine("Do " + mergePattern.Count + " merge commands");
//Transfers components between constrained groups.
foreach (KeyValuePair <CustomRigidGroup, CustomRigidGroup> pair in mergePattern)
{
pair.Value.occurrences.AddRange(pair.Key.occurrences); //Transfers key components to related value.
pair.Key.occurrences.Clear();
pair.Value.grounded = pair.Value.grounded || pair.Key.grounded; //Is it possible for the key to be grounded? Would there have to be a loop of groups?
}
Console.WriteLine("Resolve broken joints");
//Goes through each joint and sees if it was merged. If it was, it attaches the group left behind to the group that was merged into.
foreach (CustomRigidJoint joint in results.joints)
{
CustomRigidGroup updatedGroup = null; //Stores the group that the previous groupOne/Two was merged into.
if (mergePattern.TryGetValue(joint.groupOne, out updatedGroup))
{
joint.groupOne = updatedGroup;
}
if (mergePattern.TryGetValue(joint.groupTwo, out updatedGroup))
{
joint.groupTwo = updatedGroup;
}
}
Console.WriteLine("Creating planned skeletal joints");
foreach (PlannedJoint pJoint in plannedJoints)
{
SkeletalJoint_Base sJ = SkeletalJoint.Create(pJoint.joint, pJoint.parentNode.group);
pJoint.parentNode.AddChild(sJ, pJoint.node);
}
Console.WriteLine("Cleanup remainders");
CleanMeaningless(results);
return(baseRoot);
}
/// <summary>
/// Initializes physical robot based off of robot directory.
/// </summary>
/// <param name="directory">folder directory of robot</param>
/// <returns></returns>
public bool InitializeRobot(string directory, MainState source)
{
RobotIsMecanum = false;
if (RobotIsMixAndMatch)
{
wheelPath = RobotTypeManager.WheelPath;
wheelFriction = RobotTypeManager.WheelFriction;
wheelLateralFriction = RobotTypeManager.WheelLateralFriction;
wheelRadius = RobotTypeManager.WheelRadius;
wheelMass = RobotTypeManager.WheelMass;
RobotIsMecanum = RobotTypeManager.IsMecanum;
}
#region Robot Initialization
RobotDirectory = directory;
//Deletes all nodes if any exist, take the old node transforms out from the robot object
int childCount = transform.childCount;
for (int i = childCount - 1; i >= 0; i--)
{
Transform child = transform.GetChild(i);
//If this isn't done, the game object is destroyed but the parent-child transform relationship remains!
child.parent = null;
Destroy(child.gameObject);
}
//Detach and destroy all sensors on the original robot
SensorManager sensorManager = GameObject.Find("SensorManager").GetComponent <SensorManager>();
sensorManager.ResetSensorLists();
//Removes Driver Practice component if it exists
if (dpmRobot != null)
{
Destroy(dpmRobot);
}
mainState = source; //stores the main state object
transform.position = robotStartPosition; //Sets the position of the object to the set spawn point
if (!File.Exists(directory + "\\skeleton.bxdj"))
{
return(false);
}
//Loads the node and skeleton data
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new RigidNode(guid));
};
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
rootNode = BXDJSkeleton.ReadSkeleton(directory + "\\skeleton.bxdj");
rootNode.ListAllNodes(nodes);
//Initializes the wheel variables
int numWheels = nodes.Count(x => x.HasDriverMeta <WheelDriverMeta>() && x.GetDriverMeta <WheelDriverMeta>().type != WheelType.NOT_A_WHEEL);
float collectiveMass = 0f;
//Initializes the nodes and creates joints for the robot
if (RobotIsMixAndMatch && !RobotIsMecanum) //If the user is in MaM and the robot they select is not mecanum, create the nodes and replace the wheel meshes to match those selected
{
//Load Node_0, the base of the robot
RigidNode node = (RigidNode)nodes[0];
node.CreateTransform(transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName, true, wheelMass))
{
Debug.Log("Robot not loaded!");
return(false);
}
node.CreateJoint(numWheels, RobotIsMixAndMatch);
if (node.PhysicalProperties != null)
{
collectiveMass += node.PhysicalProperties.mass;
}
if (node.MainObject.GetComponent <BRigidBody>() != null)
{
node.MainObject.AddComponent <Tracker>().Trace = true;
}
//Get the wheel mesh data from the file they are stored in. They are stored as .bxda files. This may need to update if exporters/file types change.
BXDAMesh mesh = new BXDAMesh();
mesh.ReadFromFile(wheelPath + "\\node_0.bxda");
List <Mesh> meshList = new List <Mesh>();
List <Material[]> materialList = new List <Material[]>();
RigidNode wheelNode = (RigidNode)BXDJSkeleton.ReadSkeleton(wheelPath + "\\skeleton.bxdj");
Material[] materials = new Material[0];
AuxFunctions.ReadMeshSet(mesh.meshes, delegate(int id, BXDAMesh.BXDASubMesh sub, Mesh meshu)
{
meshList.Add(meshu);
materials = new Material[meshu.subMeshCount];
for (int i = 0; i < materials.Length; i++)
{
materials[i] = sub.surfaces[i].AsMaterial(true);
}
materialList.Add(materials);
}, true);
//Loads the other nodes from the original robot
for (int i = 1; i < nodes.Count; i++)
{
node = (RigidNode)nodes[i];
node.CreateTransform(transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName, true, wheelMass))
{
Debug.Log("Robot not loaded!");
return(false);
}
//If the node is a wheel, destroy the original wheel mesh and replace it with the wheels selected in MaM
if (node.HasDriverMeta <WheelDriverMeta>())
{
int chldCount = node.MainObject.transform.childCount;
for (int j = 0; j < chldCount; j++)
{
Destroy(node.MainObject.transform.GetChild(j).gameObject);
}
int k = 0;
Vector3?offset = null;
foreach (Mesh meshObject in meshList)
{
GameObject meshObj = new GameObject(node.MainObject.name + "_mesh");
meshObj.transform.parent = node.MainObject.transform;
meshObj.AddComponent <MeshFilter>().mesh = meshObject;
if (!offset.HasValue)
{
offset = meshObject.bounds.center;
}
meshObj.transform.localPosition = -offset.Value;
//Take out this line if you want some snazzy pink wheels
meshObj.AddComponent <MeshRenderer>().materials = materialList[k];
k++;
}
node.MainObject.GetComponentInChildren <MeshRenderer>().materials = materials;
}
//Create the joints that interact with physics
node.CreateJoint(numWheels, RobotIsMixAndMatch, wheelFriction, wheelLateralFriction);
if (node.HasDriverMeta <WheelDriverMeta>())
{
node.MainObject.GetComponent <BRaycastWheel>().Radius = wheelRadius;
}
if (node.PhysicalProperties != null)
{
collectiveMass += node.PhysicalProperties.mass;
}
if (node.MainObject.GetComponent <BRigidBody>() != null)
{
node.MainObject.AddComponent <Tracker>().Trace = true;
}
}
}
else //Initialize the robot as normal
{
//Initializes the nodes
foreach (RigidNode_Base n in nodes)
{
RigidNode node = (RigidNode)n;
node.CreateTransform(transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName))
{
Debug.Log("Robot not loaded!");
return(false);
}
node.CreateJoint(numWheels, RobotIsMixAndMatch);
if (node.PhysicalProperties != null)
{
collectiveMass += node.PhysicalProperties.mass;
}
if (node.MainObject.GetComponent <BRigidBody>() != null)
{
node.MainObject.AddComponent <Tracker>().Trace = true;
}
}
}
#endregion
//Get the offset from the first node to the robot for new robot start position calculation
//This line is CRITICAL to new reset position accuracy! DON'T DELETE IT!
nodeToRobotOffset = gameObject.transform.GetChild(0).localPosition - robotStartPosition;
foreach (BRaycastRobot r in GetComponentsInChildren <BRaycastRobot>())
{
r.RaycastRobot.OverrideMass = collectiveMass;
r.RaycastRobot.RootRigidBody = (RigidBody)((RigidNode)nodes[0]).MainObject.GetComponent <BRigidBody>().GetCollisionObject();
}
RotateRobot(robotStartOrientation);
RobotName = new DirectoryInfo(directory).Name;
isInitialized = true;
//Initializes Driver Practice component
dpmRobot = gameObject.AddComponent <DriverPracticeRobot>();
dpmRobot.Initialize(directory);
//Initializing robot cameras
bool hasRobotCamera = false;
//If you are getting an error referencing this line, it is likely that the Game Object "RobotCameraList" in Scene.unity does not have the RobotCameraManager script attached to it.
robotCameraManager = GameObject.Find("RobotCameraList").GetComponent <RobotCameraManager>();
//Loop through robotCameraList and check if any existing camera should attach to this robot
foreach (GameObject robotCamera in robotCameraManager.GetRobotCameraList())
{
if (robotCamera.GetComponent <RobotCamera>().robot.Equals(this))
{
//Recover the robot camera configurations
robotCamera.GetComponent <RobotCamera>().RecoverConfiguration();
hasRobotCamera = true;
}
}
//Add new cameras to the robot if there is none robot camera belong to the current robot (which means it is a new robot)
if (!hasRobotCamera)
{
//Attached to the main frame and face the front
robotCameraManager.AddCamera(this, transform.GetChild(0).transform, new Vector3(0, 0.5f, 0), new Vector3(0, 0, 0));
////Attached to main frame and face the back
robotCameraManager.AddCamera(this, transform.GetChild(0).transform, new Vector3(0, 0.5f, 0), new Vector3(0, 180, 0));
robotCameraManager.AddCamera(this, transform.GetChild(0).transform);
}
//Reads the offset position for the manipulator
if (RobotIsMixAndMatch)
{
offset = Vector3.zero;
try
{
using (TextReader reader = File.OpenText(directory + "\\position.txt"))
{
offset.x = float.Parse(reader.ReadLine());
offset.y = float.Parse(reader.ReadLine());
offset.z = float.Parse(reader.ReadLine());
}
} catch
{
offset = Vector3.zero;
}
}
return(true);
}
/// <summary>
/// Loads and initializes the physical manipulator object (used in Mix and Match mode)
/// </summary>
/// <param name="directory">Folder directory of the manipulator</param>
/// <param name="robotGameObject">GameObject of the robot the manipulator will be attached to</param>
public bool InitializeManipulator(string directory, GameObject robotGameObject)
{
if (robotGameObject == null)
{
robotGameObject = GameObject.Find("Robot");
}
ManipulatorObject = new GameObject("Manipulator");
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new RigidNode(guid));
};
List <RigidNode_Base> nodes = new List <RigidNode_Base>();
//TO-DO: Read .robot instead (from the new exporters if they are implemented). Maybe need a RobotSkeleton class
manipulatorNode = BXDJSkeleton.ReadSkeleton(directory + "\\skeleton.bxdj");
manipulatorNode.ListAllNodes(nodes);
int numWheels = nodes.Count(x => x.HasDriverMeta <WheelDriverMeta>() && x.GetDriverMeta <WheelDriverMeta>().type != WheelType.NOT_A_WHEEL);
float collectiveMass = 0f;
//Load node_0 for attaching manipulator to robot
RigidNode node = (RigidNode)nodes[0];
node.CreateTransform(ManipulatorObject.transform);
if (!node.CreateMesh(directory + "\\" + node.ModelFileName))
{
Debug.Log("Robot not loaded!");
UnityEngine.Object.Destroy(ManipulatorObject);
return(false);
}
GameObject robot = robotGameObject;
//Set the manipulator transform to match with the position of node_0 of the robot. THIS ONE ACTUALLY DOES SOMETHING: LIKE ACTUALLY
Vector3 manipulatorTransform = robotStartPosition + offset;
Debug.Log("Node Com Offset" + node.ComOffset);
ManipulatorObject.transform.position = manipulatorTransform;
node.CreateManipulatorJoint(robot);
node.MainObject.AddComponent <Tracker>().Trace = true;
Tracker t = node.MainObject.GetComponent <Tracker>();
Debug.Log(t);
//Load other nodes associated with the manipulator
for (int i = 1; i < nodes.Count; i++)
{
RigidNode otherNode = (RigidNode)nodes[i];
otherNode.CreateTransform(ManipulatorObject.transform);
if (!otherNode.CreateMesh(directory + "\\" + otherNode.ModelFileName))
{
Debug.Log("Robot not loaded!");
UnityEngine.Object.Destroy(ManipulatorObject);
return(false);
}
otherNode.CreateJoint(numWheels, RobotIsMixAndMatch);
otherNode.MainObject.AddComponent <Tracker>().Trace = true;
t = otherNode.MainObject.GetComponent <Tracker>();
Debug.Log(t);
}
foreach (BRaycastRobot r in ManipulatorObject.GetComponentsInChildren <BRaycastRobot>())
{
r.RaycastRobot.OverrideMass = collectiveMass;
}
RotateRobot(robotStartOrientation);
this.RobotHasManipulator = true;
return(true);
}
public void CreateJoint(int numWheels)
{
if (joint != null || GetSkeletalJoint() == null)
{
return;
}
switch (GetSkeletalJoint().GetJointType())
{
case SkeletalJointType.ROTATIONAL:
if (this.HasDriverMeta <WheelDriverMeta>() && this.GetDriverMeta <WheelDriverMeta>().type != WheelType.NOT_A_WHEEL)
{
RigidNode parent = (RigidNode)GetParent();
if (parent.MainObject.GetComponent <BRaycastRobot>() == null)
{
BRaycastRobot robot = parent.MainObject.AddComponent <BRaycastRobot>();
robot.NumWheels = numWheels;
if (MixAndMatchMode.isMixAndMatchMode)
{
robot.Friction = PlayerPrefs.GetFloat("wheelFriction", 1);
}
}
WheelType wheelType = this.GetDriverMeta <WheelDriverMeta>().type;
MainObject.AddComponent <BRaycastWheel>().CreateWheel(this);
MainObject.transform.parent = parent.MainObject.transform;
}
else
{
RotationalJoint_Base rNode = (RotationalJoint_Base)GetSkeletalJoint();
BHingedConstraintEx hc = (BHingedConstraintEx)(joint = ConfigJoint <BHingedConstraintEx>(rNode.basePoint.AsV3() - ComOffset, rNode.axis.AsV3(), AxisType.X));
Vector3 rAxis = rNode.axis.AsV3().normalized;
hc.axisInA = rAxis;
hc.axisInB = rAxis;
if (hc.setLimit = rNode.hasAngularLimit)
{
hc.lowLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitHigh;
hc.highLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitLow;
}
hc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
}
break;
case SkeletalJointType.CYLINDRICAL:
CylindricalJoint_Base cNode = (CylindricalJoint_Base)GetSkeletalJoint();
B6DOFConstraint bc = (B6DOFConstraint)(joint = ConfigJoint <B6DOFConstraint>(cNode.basePoint.AsV3() - ComOffset, cNode.axis.AsV3(), AxisType.X));
bc.linearLimitLower = new Vector3(cNode.linearLimitStart * 0.01f, 0f, 0f);
bc.linearLimitUpper = new Vector3(cNode.linearLimitEnd * 0.01f, 0f, 0f);
bc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
break;
case SkeletalJointType.LINEAR:
LinearJoint_Base lNode = (LinearJoint_Base)GetSkeletalJoint();
Vector3 lAxis = lNode.axis.AsV3().normalized;
// TODO: Figure out how to make a vertical slider?
BSliderConstraint sc = (BSliderConstraint)(joint = ConfigJoint <BSliderConstraint>(lNode.basePoint.AsV3() - ComOffset, lNode.axis.AsV3(), AxisType.X));
if (lAxis.x < 0)
{
lAxis.x *= -1f;
}
if (lAxis.y < 0)
{
lAxis.y *= -1f;
}
if (lAxis.z < 0)
{
lAxis.z *= -1f;
}
sc.localConstraintAxisX = lAxis;
sc.localConstraintAxisY = new Vector3(lAxis.y, lAxis.z, lAxis.x);
sc.lowerLinearLimit = lNode.linearLimitLow * 0.01f;
sc.upperLinearLimit = lNode.linearLimitHigh * 0.01f;
sc.lowerAngularLimitRadians = 0f;
sc.upperAngularLimitRadians = 0f;
sc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
bool b = this.HasDriverMeta <ElevatorDriverMeta>();
if (GetSkeletalJoint().cDriver != null)
{
if (GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
MainObject.GetComponent <BRigidBody>().mass *= 2f;
}
}
break;
}
}
/// <summary>
/// Updates the motors on the manipulator in mix and match mode. Called every frame.
/// </summary>
/// <param name="skeleton"></param>
/// <param name="dioModules"></param>
/// <param name="controlIndex"></param>
public static void UpdateManipulatorMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex)
{
float[] pwm;
float[] can;
if (dioModules[0] != null)
{
pwm = dioModules[0].pwmValues;
can = dioModules[0].canValues;
}
else
{
pwm = new float[10];
can = new float[10];
}
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SPEED_ARROW_PWM);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SPEED_ARROW_PWM);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SPEED_ARROW_PWM);
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
if (raycastWheel != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
raycastWheel.ApplyForce(pwm[i]);
}
}
if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null)
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
if (rigidNode.HasDriverMeta <WheelDriverMeta>())
{
maxSpeed = WHEEL_MAX_SPEED;
impulse = WHEEL_MOTOR_IMPULSE;
friction = WHEEL_COAST_FRICTION;
}
else
{
maxSpeed = HINGE_MAX_SPEED;
impulse = HINGE_MOTOR_IMPULSE;
friction = HINGE_COAST_FRICTION;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MAX_SLIDER_FORCE;
sc.TargetLinearMotorVelocity = pwm[i] * MAX_SLIDER_SPEED;
}
}
}
}
}
}
