public IEnumerator SetRootJoint(BodyData.Joints rootJoint)
{
Debug.Log("SetRootJoint: " + rootJoint);
string jointName = rootJoint.ToString() + "Joint";
if (!joints.ContainsKey(jointName))
{
Debug.LogError("SetRootJoint got illegal joint name: " + jointName);
yield return(null);
}
JointController jointC = joints [jointName];
Transform joint = jointC.transform;
Transform parent = joint.parent;
Transform prevParent = joint;
while (parent != transform && parent != null)
{
Transform nextParent = parent.parent;
joint.SetParent(nextParent);
parent.SetParent(prevParent);
foreach (Transform child in parent)
{
if ((child.gameObject.tag == "BodyPart") && (child.GetComponent <BodyPartController>() != null))
{
child.GetComponent <BodyPartController>().TrySetParent(prevParent);
}
}
prevParent = parent;
parent = nextParent;
}
}
void AssignTaskToJoints()
{
for (int i = 0; i < timeSortedJoints.Count; i++)
{
Joint JD = timeSortedJoints[i];
for (int j = 0; j < timeSortedJoints[i].bodyName.Length; j++)
{
JointController JC = AllJoints[(HumanBodyBones)Enum.Parse(typeof(HumanBodyBones), JD.bodyName[j], true)];
JC.Task(JD.start, JD.end, JD.rotateCap, JD.hold);
}
}
for (int i = 0; i < timeSortedHipPos.Count; i++)
{
HipJoint HJ = timeSortedHipPos[i];
JointController JC = AllJoints[(HumanBodyBones)Enum.Parse(typeof(HumanBodyBones), HJ.bodyName, true)];
JC.Task2(HJ.start, HJ.end, HJ.addPos);
}
for (int i = 0; i < timeSortedSupportJoint.Count; i++)
{
Support SP = timeSortedSupportJoint[i];
JointController JC = AllJoints[(HumanBodyBones)Enum.Parse(typeof(HumanBodyBones), SP.bodyName, true)];
JC.Task3(SP.start, SP.end, SP.rotateTo, SP.hold);
}
for (int i = 0; i < timeSortedGesture.Count; i++)
{
gestureJoint GJ = timeSortedGesture[i];
JointController JC = AllJoints[(HumanBodyBones)Enum.Parse(typeof(HumanBodyBones), GJ.fingerName, true)];
JC.Task4(GJ.start, GJ.end, GJ.position, GJ.hold);
}
}
void ProcessAndCreateJoints()
{
for (int i = 0; i < 55; i++)
{
string check;
Transform BoneTrans = Anim.GetBoneTransform((HumanBodyBones)i);
if (BoneTrans != null)
{
//GameObject controller = Instantiate(JointControllerPrefab); //Create Controller Prefab
//Vector3 ConPos = BoneTrans.position; //Get position of Specific HumanBody Bones
//ConPos.z += 1; //1 Vector.z unit away from model
//controller.transform.position = ConPos; // Set Controller Prefab position 1 Vector.z unit away from specific HumanBody Bones
//controller.transform.rotation = BoneTrans.rotation;
//BoneTrans.transform.SetParent(controller.transform);
BoneTrans.name = ((HumanBodyBones)i).ToString() + " JC";
if (((HumanBodyBones)i).ToString() == "Hips")
{
hipXValue = BoneTrans.transform.position.x;
}
JointController JC = BoneTrans.gameObject.AddComponent <JointController>();
//JC.JointToControllTrans = BoneTrans; //JC.JointToControllTrans = Anim.GetBoneTransform((HumanBodyBones)i)
AllJoints.Add((HumanBodyBones)i, JC);
check = ((HumanBodyBones)i).ToString();
//if(check[0] == 'L')
if (((HumanBodyBones)i).ToString().StartsWith("Left"))
{
BoneTrans.gameObject.tag = "Left";
}
}
}
}
public KinectTester()
{
InitializeComponent();
sensor = new SmartKinectSensor();
renderCanvas = new RenderCanvas(TimeSpan.FromSeconds(10));
sensor.EnableSkeletonRenderingSensor();
renderCanvas.ImageRendered += DisplayRenderedImage;
capturedLabel.Visible = false;
jointController = new JointController();
gestureController = new GestureController();
skeletonRender = new SkeletonRenderer(sensor.Sensor);
sensor.SkeletonController.AddFunction(jointController);
sensor.SkeletonController.AddFunction(gestureController);
sensor.SkeletonController.AddFunction(skeletonRender);
skeletonRender.SkeletonRendered += renderCanvas.SkeletonFrameCaptured;
gestureController.GestureCaptured += GestureCaptured;
jointController.JointTrackingCaptured += JointTrackingCaptured;
ArduinoSerialPort serialPort = new ArduinoSerialPort("COM5", 115200);
serialPort.Open();
bodyTracker = new KinectBodyTracker(sensor, serialPort);
GestureBuilderForm test = new GestureBuilderForm(sensor);
test.Show();
// InitializeArduino();
}
public KinectTester()
{
InitializeComponent();
sensor = new SmartKinectSensor();
renderCanvas = new RenderCanvas(TimeSpan.FromSeconds(10));
sensor.EnableSkeletonRenderingSensor();
renderCanvas.ImageRendered += DisplayRenderedImage;
capturedLabel.Visible = false;
jointController = new JointController();
gestureController = new GestureController();
skeletonRender = new SkeletonRenderer(sensor.Sensor);
sensor.SkeletonController.AddFunction(jointController);
sensor.SkeletonController.AddFunction(gestureController);
sensor.SkeletonController.AddFunction(skeletonRender);
skeletonRender.SkeletonRendered += renderCanvas.SkeletonFrameCaptured;
gestureController.GestureCaptured += GestureCaptured;
jointController.JointTrackingCaptured += JointTrackingCaptured;
ArduinoSerialPort serialPort = new ArduinoSerialPort("COM5", 115200);
serialPort.Open();
bodyTracker = new KinectBodyTracker(sensor, serialPort);
GestureBuilderForm test = new GestureBuilderForm(sensor);
test.Show();
// InitializeArduino();
}
private void ConnectSkeletonController()
{
JointController jointController = new JointController();
skeletonController.AddFunction(jointController);
jointController.JointTrackingCaptured += jointController_JointTrackingCaptured;
}
public void ReleaseGrapple()
{
if (activeJoint != null)
{
JointController jointController = activeJoint.GetComponent <JointController>();
jointController.ReleaseLastJointAnchor();
}
}
public void FireGrapple()
{
ReleaseGrapple();
activeJoint = Instantiate(HingePoint, gameObject.transform.position, Quaternion.identity) as GameObject;
JointController jointController = activeJoint.GetComponent <JointController>();
jointController.SetLastJointAnchor(rb);
jointController.LaunchFirstJoint(grappleThrowForce, gameObject.transform.right, grappleLockTime);
}
void Update()
{
for (int i = 0; i < motors.Length; i++)
{
Motor m = motors[i];
JointController joint = m.gameObject.GetComponent <JointController>();
joint.RotateTo(m.targetPosition);
m.presentPosition = joint.GetPresentPosition();
}
}
public void FireGrapple()
{
//Vector3 mousePosition = Input.mousePosition;
//mousePosition = Camera.main.ScreenToWorldPoint(mousePosition);
//mousePosition.z = 0;
//if (Vector2.Distance(gameObject.transform.position, mousePosition) > maxGrappleDistance)
// return;
ReleaseGrapple();
activeJoint = Instantiate(HingePoint, gameObject.transform.position, Quaternion.identity) as GameObject;
JointController jointController = activeJoint.GetComponent <JointController>();
jointController.SetLastJointAnchor(rb);
jointController.LaunchFirstJoint(grappleThrowForce, gameObject.transform.right, grappleLockTime);
}
// Start is called before the first frame update
void Start()
{
Academy.Instance.OnEnvironmentReset += () =>
{
Reset();
};
// Initialize joint controllers
leftShoulder = new JointController("Left shoulder", leftShoulderJoint, new Vector3(-999, -999, -50), new Vector3(999, 999, 50));
leftUpperArm = new JointController("Left upper arm", leftUpperArmJoint, new Vector3(-999, -999, -999), new Vector3(999, 999, 999));
leftElbow = new JointController("Left elbow", leftElbowJoint, new Vector3(-50, -999, -999), new Vector3(50, 999, 999));
parameterScript = parameterObject.GetComponent <Parameters>();
// Configure joint angles based on condition
setArmCondition();
}
/// <summary>
/// Initial setup, called at startup.
/// </summary>
/// <remarks>
/// In future versions of ML-agents (> 0.14), InitializeAgent should be replaced by Initialize.
/// </remarks>
public override void InitializeAgent()
{
base.InitializeAgent();
// Initialize joint controllers
leftShoulder = new JointController("Left shoulder", leftShoulderJoint, new Vector3(-999, -999, -999), new Vector3(999, 999, 999)); //new Vector3(-999, -999, -25), new Vector3(999, 999, 25));
leftUpperArm = new JointController("Left upper arm", leftUpperArmJoint, new Vector3(-999, -999, -999), new Vector3(999, 999, 999));
leftElbow = new JointController("Left elbow", leftElbowJoint, new Vector3(-999, -999, -999), new Vector3(999, 999, 999)); //new Vector3(-25, -999, -999), new Vector3(25, 999, 999));
head = new JointController("Head", headJoint, new Vector3(-999, -999, -999), new Vector3(999, 999, 999));
// Retrieve the rubberArm and visuo-tactile stimulation objects from the environment
rubberArmController = rubberArm.GetComponent <RubberArmController>();
ballScript = ballHandler.GetComponent <BallHandler>();
vibScript = vibHandler.GetComponent <VibHandler>();
// Set the head rotation (and subsequently the camera perspective)
head.SetRelativeJointAngles(new Vector3(15f, -20f));
}
void CreateJoints()
{
JointControllers = new List <JointController>();
HumanBodyBonesTrackData[] BonesToTrack = BodyManager.instance.BonesToTrack;
for (int i = 0; i < BonesToTrack.Length; i++)
{
if (animatorComponent.GetBoneTransform(BonesToTrack[i].Joint) != null)
{
GameObject JointObject = Instantiate(JointControllerPrefab, Vector3.zero, Quaternion.identity);
JointController JC = JointObject.GetComponent <JointController>();
JC.SetTrackingData(animatorComponent, BonesToTrack[i], AnimatorToCompareTracker, this);
JointControllers.Add(JC);
}
else
{
Debug.Log(gameObject.name + " doesn't have " + BonesToTrack[i].ToString() + " Bone", gameObject);
}
}
}
private KineticHierarchyController BuildJoint(KineticHierarchyController parent, string part_name, int id,
XmlNode joint_xml, XmlUtils.Defaults defaults)
{
string joint_name = XmlUtils.GetString(joint_xml, "name", string.Format("{0}_joint_{1}", part_name, id));
XmlUtils.Defaults joint_defaults = defaults.GetSubclass("joint");
JointController joint = SceneUtils.InstantiateWithController <JointController>(joint_name);
joint.Initialize(parent, joint_name,
XmlUtils.GetVector3WithDefaults(joint_xml, joint_defaults, "pos", Vector3.zero),
XmlUtils.GetRotationWithDefaults(joint_xml, joint_defaults, Quaternion.identity),
XmlUtils.GetVector3WithDefaults(joint_xml, joint_defaults, "axis", Vector3.up),
XmlUtils.GetVector2WithDefaults(joint_xml, joint_defaults, "range",
new Vector2(float.MinValue, float.MaxValue)),
XmlUtils.GetStringWithDefaults(joint_xml, joint_defaults, "type", null));
return(joint);
}
// Use this for initialization
void Start()
{
JointController jc = GameController.FindObjectOfType <JointController>();
CharacterJoint cj = GetComponent <CharacterJoint>();
float rotation = jc.maxRotation; //+ (jc.maxRotation * Random.Range(-50, 50)/100.0f * jc.randomness);
float mass = jc.mass; // +(jc.mass * Random.Range(-50, 50) / 100.0f * jc.randomness);
float drag = jc.drag; // +(jc.drag * Random.Range(-50, 50) / 100.0f * jc.randomness);
float angularDrag = jc.angularDrag; // +(jc.angularDrag * Random.Range(-50, 50) / 100.0f * jc.randomness);
float massModif = Mathf.Sqrt(massModifier);
SoftJointLimit limit = cj.highTwistLimit;
limit.limit = rotation;
limit.spring = jc.spring;
limit.damper = jc.damping;
cj.highTwistLimit = limit;
limit = cj.lowTwistLimit;
limit.limit = -rotation;
limit.spring = jc.spring;
limit.damper = jc.damping;
cj.lowTwistLimit = limit;
limit = cj.swing1Limit;
limit.limit = rotation;
limit.spring = jc.spring;
limit.damper = jc.damping;
cj.swing1Limit = limit;
limit = cj.swing2Limit;
limit.limit = rotation;
limit.spring = jc.spring;
limit.damper = jc.damping;
cj.swing2Limit = limit;
Rigidbody rigid = GetComponent <Rigidbody>();
rigid.mass = mass * massModif;
rigid.drag = drag;
rigid.angularDrag = angularDrag;
}
private void Remove()
{
if (_type == Type.Rectangle)
{
var listLines = GetComponent <RectangleInfo>().listLines;
while (listLines.Count > 0)
{
_destroyingLine = listLines[0];
LineDestroy(_destroyingLine.startRectangleInfo.listLines, _destroyingLine);
LineDestroy(_destroyingLine.finishRectangleInfo.listLines, _destroyingLine);
}
Destroy(gameObject);
}
else
{
_destroyingLine = GetComponent <JointController>();
LineDestroy(_destroyingLine.startRectangleInfo.listLines, _destroyingLine);
LineDestroy(_destroyingLine.finishRectangleInfo.listLines, _destroyingLine);
Destroy(gameObject);
}
}
//Subida de la cadena
public void collectChain()
{
if (i != 23)
{
//Por cada eslabon se habilita el movimiento poniendolos kinematic
chainRb = chain[i].GetComponent <Rigidbody>();
jc = chain[i].GetComponent <JointController>();
chainRb.useGravity = false;
chainRb.isKinematic = true;
//Se mueven hasta el punto de ascenso
chain[i].transform.position = Vector3.MoveTowards(chain[i].transform.position, pointAscend.position, step);
//Cuando llega al punto de ascenso se desactiva y se asignan los valores correspondientes al siguiente eslabon
if (chain[i].transform.position == pointAscend.position)
{
chain[i].SetActive(false);
jc.assignNext();
}
}
}
// Update all joints from a list of qposes.
public bool UpdateState(IList <float> state)
{
for (int i = 0; i < Mathf.Min(state.Count, mapping_.Length); ++i)
{
JointController joint = mapping_[i];
if (joint != null)
{
switch (joint.joint_type_)
{
case JointController.JointType.Hinge:
case JointController.JointType.Slide:
joint.UpdateJoint(state[i]);
break;
case JointController.JointType.Ball:
joint.UpdateJoint(GetQuaternion(state, i));
break;
}
}
}
return(true);
}
//If was left click on the Rectangle and then was right click on another Rectangle, then between these Rectangles makes the Line
public void OnPointerClick(PointerEventData eventData)
{
if (eventData.button == PointerEventData.InputButton.Left)
{
startRectangle = this;
}
else if (eventData.button == PointerEventData.InputButton.Right &&
startRectangle != null &&
startRectangle != gameObject &&
!_joinedLineExist)
{
GameObject newLine = Instantiate(linePrefab, new Vector3().normalized, Quaternion.identity, canvasTransform);
JointController newJointController = newLine.GetComponent <JointController>();
newJointController.startRectangle = startRectangle.transform;
newJointController.finishRectangle = transform;
newJointController.startRectangleInfo = startRectangle;
newJointController.finishRectangleInfo = this;
startRectangle.listLines.Add(newJointController);
listLines.Add(newJointController);
}
}
//Bajada de la cadena
public void releaseChain()
{
//Por cada eslabon se habilita su gravedad, activandola
chainRb = chain[i].GetComponent <Rigidbody>();
jc = chain[i].GetComponent <JointController>();
chainRb.useGravity = true;
chainRb.isKinematic = false;
if (i > 0)
{
//Se mueve el objeto auxiliar hasta el punto de descenso
auxObject.transform.position = Vector3.MoveTowards(auxObject.transform.position, pointDescend.position, step);
//Durante el movimiento
if (auxObject.transform.position.y <= pointDescend.position.y)
{
//Activamos el eslabon previo y se asignan los valores correspondientes
auxObject.transform.position = startPosition;
chain[i - 1].SetActive(true);
jc.assignPrevious();
}
}
}
private void LineDestroy(List <JointController> currentListLines, JointController destroyingLine)
{
currentListLines.Remove(destroyingLine);
Destroy(destroyingLine.gameObject);
}
public void AttachJointController(float kp, float tolerance)
{
JointController = new JointController(kp, tolerance, Links.Count, Links.Select(x => x.Type).ToArray());
}
private void ConnectSkeletonController()
{
JointController jointController = new JointController();
skeletonController.AddFunction(jointController);
jointController.JointTrackingCaptured += jointController_JointTrackingCaptured;
}
