private float calcSPD(float currTheta, float targetTheta, float currVel, JsonPDController pd) { float thetaErr = targetTheta - currTheta; float result = pd.Kp * thetaErr - pd.Kd * currVel; return(result); }
public float[] getTau(JsonAction action, Character character) { List <JsonPDController> jsonPDs = character.mjsonObject.PDControllers; List <GameObject> bodies = character.mBodies; int numJoint = jsonPDs.Count; float[] targetTau = new float[numJoint]; float[] tauErr = new float[numJoint]; for (int i = 1; i < numJoint; i++) { targetTau [i] = 0; JsonPDController pd = jsonPDs [i]; GameObject body = bodies [i]; HingeJoint joint = bodies [i].GetComponent <HingeJoint> (); float currTheta = Mathf.Deg2Rad * joint.angle; float currVel = Mathf.Deg2Rad * joint.velocity; float targetTheta; // if current joint is controlled by action, use that theta if (pd.Name.Contains("spine")) { targetTheta = getTargetTheta(action.StateParams, "SpineCurve"); } else if (CONTROL_PARAMS.Contains(pd.Name)) { targetTheta = getTargetTheta(action.StateParams, pd.Name); } else { targetTheta = pd.TargetTheta; } JointSpring spring = joint.spring; spring.targetPosition = Mathf.Rad2Deg * targetTheta; joint.spring = spring; //targetTau[i] += calcSPD(currTheta, targetTheta, currVel, pd); int parent = character.mjsonObject.Skeleton.Joints[i].Parent; float mass = character.mjsonObject.BodyDefs [i].Mass; calcGravity(ref targetTau, body, joint, mass, i, parent); // targetTau[i] += calcVirtual tauErr [i] = targetTau [i] - mPastTau [i]; mPastTau [i] = targetTau [i]; } return(tauErr); }
public Character(TextAsset jsonText, GameObject refCube) { mjsonObject = JsonUtility.FromJson <JsonObject> (jsonText.text); mBodies = new List <GameObject> (); // make root JsonJoint joint = mjsonObject.Skeleton.Joints [0]; JsonBodyDef body = mjsonObject.BodyDefs [0]; GameObject root = GameObject.Instantiate(refCube); root.name = "root"; Mesh mesh = root.GetComponent <MeshFilter> ().mesh; mesh.vertices = StretchMesh(mesh.vertices, body.Param0, body.Param1); mesh.RecalculateBounds(); root.GetComponent <MeshCollider> ().sharedMesh = mesh; mBodies.Add(root); for (int i = 1; i < mjsonObject.Skeleton.Joints.Count; i++) { joint = mjsonObject.Skeleton.Joints [i]; body = mjsonObject.BodyDefs [i]; // info of parent body & joint int parentIdx = joint.Parent; JsonJoint parentJoint = mjsonObject.Skeleton.Joints [parentIdx]; JsonBodyDef parentBody = mjsonObject.BodyDefs [parentIdx]; GameObject parent = GameObject.Find(parentBody.Name); //Vector3 scale = new Vector3 (body.Param0, body.Param1, 0.1f); //refCube.transform.localScale = scale; GameObject obj = (GameObject)GameObject.Instantiate(refCube); obj.name = body.Name; // set position of body part Vector3 position = new Vector3(body.AttachX, body.AttachY) - new Vector3(parentBody.AttachX, parentBody.AttachY); obj.transform.position = parent.transform.position + position + new Vector3(joint.AttachX, joint.AttachY); // set joint HingeJoint hingeJoint = obj.AddComponent <HingeJoint> (); hingeJoint.connectedBody = parent.GetComponent <Rigidbody>(); hingeJoint.autoConfigureConnectedAnchor = false; hingeJoint.anchor = -new Vector3(body.AttachX, body.AttachY); hingeJoint.connectedAnchor = parent.transform.InverseTransformPoint(obj.transform.TransformPoint(hingeJoint.anchor)); hingeJoint.autoConfigureConnectedAnchor = true; hingeJoint.axis = new Vector3(0, 0, 1); hingeJoint.useSpring = true; JointSpring spring = new JointSpring(); JsonPDController pd = mjsonObject.PDControllers [i]; spring.targetPosition = Mathf.Rad2Deg * pd.TargetTheta; spring.spring = 20 * pd.Kd; spring.damper = 0.1f * pd.Kp; hingeJoint.spring = spring; if (joint.LimLow < joint.LimHigh) { hingeJoint.useLimits = true; JointLimits limit = new JointLimits(); limit.max = Mathf.Rad2Deg * joint.LimHigh; limit.min = Mathf.Rad2Deg * joint.LimLow; hingeJoint.limits = limit; } // resize mesh mesh = obj.GetComponent <MeshFilter> ().mesh; mesh.vertices = StretchMesh(mesh.vertices, body.Param0, body.Param1); mesh.RecalculateBounds(); obj.GetComponent <MeshCollider> ().sharedMesh = mesh; obj.GetComponent <Rigidbody> ().mass = body.Mass; mBodies.Add(obj); } for (int i = 0; i < mjsonObject.Skeleton.Joints.Count; i++) { for (int j = i; j < mjsonObject.Skeleton.Joints.Count; j++) { Physics.IgnoreCollision(mBodies [i].GetComponent <MeshCollider>(), mBodies [j].GetComponent <MeshCollider>()); } } refCube.SetActive(false); }