public Vector3 drive(Quaternion p_current, Quaternion p_goal) { // To get quaternion "delta", rotate by the inverse of current // to get to the origin, then multiply by goal rotation to get "what's left" // The resulting quaternion is the "delta". Quaternion error = p_goal * Quaternion.Inverse(p_current); // Separate angle and axis, so we can feed the axis-wise // errors to the PIDs. float a; Vector3 dir; error.ToAngleAxis(out a, out dir); // Get torque Vector3 vec = m_pid.drive(a * dir, Mathf.Max(0.0001f, Time.deltaTime)); return(vec); // Note, these are 3 PIDs }
// Update is called once per frame void Update() { // To get quaternion "delta", rotate by the inverse of current // to get to the origin, then multiply by goal rotation to get "what's left" // The resulting quaternion is the "delta". Quaternion error = goal.rotation * Quaternion.Inverse(transform.rotation); //Debug.Log(error.ToString()); //transform.rotation *= error; float a; Vector3 dir; error.ToAngleAxis(out a, out dir); // Get scalars //float x, y, z; //x = m_pidX.drive(a*dir.x, Time.deltaTime); //y = m_pidY.drive(a*dir.y, Time.deltaTime); //z = m_pidZ.drive(a*dir.z, Time.deltaTime); torque = m_pid.drive(a * dir, Time.deltaTime); // //torque = new Vector3(x, y, z); Debug.Log(torque.ToString()); }
// Update is called once per frame void FixedUpdate() { rigidbody.AddTorque(m_driver.drive(transform.rotation, m_goal.rotation, Time.deltaTime)); }
void calculate() { int kneeFlip = 1; // Retrieve the current wanted foot position Vector3 footPos = Vector3.zero; if (m_foot != null) { footPos = new Vector3(0.0f, m_foot.position.y, -(m_foot.position.z - transform.position.z)); } else if (m_legFrame != null) { // get non-corrected foot pos here footPos = new Vector3(0.0f, m_legFrame.getReferenceFootPos((int)m_legType).y, -(m_legFrame.getReferenceFootPos((int)m_legType).z - m_legFrame.getOptimalProgress()) /*- (m_legFrame.getOptimalProgress() + m_legFrame.getReferenceLiftPos((int)m_legType).z)) + */ /*- transform.position.z*/); //new Vector3(0.0f, m_legFrame.getGraphedFootPos((int)m_legType), 0.0f/*m_legFrame.m_footTarget[(int)m_legType].z-m_legFrame.transform.position.z*/); } //footPos = m_legFrame.m_footTarget[(int)m_legType]; //footPos = new Vector3(footPos.x, 0.0f, footPos.z); //footPos -= m_legFrame.transform.position; Vector3 upperLegLocalPos /* = (m_upperLeg.position - m_legFrame.transform.position)*/; upperLegLocalPos = new Vector3(0.0f, m_upperLeg.localScale.y + m_lowerLeg.localScale.y + 0.2f, 0.0f); Debug.DrawLine(footPos, footPos + Vector3.up, Color.black); // Vector between foot and hip Vector3 topToFoot = upperLegLocalPos - footPos; topToFoot = new Vector3(topToFoot.x, -topToFoot.y, topToFoot.z); //Debug.DrawLine(m_upperLeg.position, m_upperLeg.position+topToFoot,Color.black); // This ik calc is in 2d, so eliminate rotation // Use the coordinate system of the leg frame as // in the paper /*if (m_legFrame != null) * topToFoot = m_legFrame.transform.InverseTransformDirection(topToFoot);*/ //Debug.DrawLine(m_upperLeg.position, m_upperLeg.position + topToFoot, Color.yellow); topToFoot.x = 0.0f; // squish x axis //Debug.DrawLine(m_upperLeg.position, m_upperLeg.position + topToFoot, Color.yellow*2.0f); // float toFootLen = topToFoot.magnitude * 1.0f; float upperLegAngle = 0.0f; float lowerLegAngle = 0.0f; float uB = m_upperLeg.localScale.y * 1.0f; // the length of the legs float lB = m_lowerLeg.localScale.y * 1.0f; //Debug.Log(uB); // first get offset angle beetween foot and axis float offsetAngle = Mathf.Atan2(topToFoot.y, topToFoot.z); // If dist to foot is shorter than combined leg length //bool straightLeg = false; if (toFootLen < uB + lB) { float uBS = uB * uB; float lBS = lB * lB; float hBS = toFootLen * toFootLen; // law of cosines for first angle upperLegAngle = (float)(kneeFlip) * Mathf.Acos((hBS + uBS - lBS) / (2.0f * uB * toFootLen)) + offsetAngle; // second angle Vector2 newLeg = new Vector2(uB * Mathf.Cos(upperLegAngle), uB * Mathf.Sin(upperLegAngle)); Vector3 kneePosT = upperLegLocalPos + new Vector3(0.0f, newLeg.y, newLeg.x); //Debug.DrawLine(m_upperLeg.position, kneePosT,Color.magenta); lowerLegAngle = Mathf.Atan2(topToFoot.y - newLeg.y, topToFoot.z - newLeg.x) - upperLegAngle; /*lowerLegAngle = acos((uBS + lBS - hBS)/(2.0f*uB*lB))-upperLegAngle;*/ } else // otherwise, straight leg { upperLegAngle = offsetAngle; Vector2 newLeg = new Vector2(uB * Mathf.Cos(upperLegAngle), uB * Mathf.Sin(upperLegAngle)); Vector3 kneePosT = upperLegLocalPos + new Vector3(0.0f, newLeg.y, newLeg.x); //Debug.DrawLine(m_upperLeg.position, kneePosT, Color.magenta); lowerLegAngle = 0.0f; //straightLeg = true; } float lowerAngleW = upperLegAngle + lowerLegAngle; m_hipAngle = upperLegAngle; m_kneeAngle = lowerAngleW; // Debug draw bones m_kneePos = new Vector3(0.0f, uB * Mathf.Sin(upperLegAngle), uB * Mathf.Cos(upperLegAngle)); m_endPos = new Vector3(0.0f, lB * Mathf.Sin(lowerAngleW), lB * Mathf.Cos(lowerAngleW)); //Debug.Log("upper angle: " + upperLegAngle); if (m_legFrame != null) { m_kneePos = upperLegLocalPos + m_kneePos /* m_legFrame.transform.TransformDirection(m_kneePos)*/; m_endPos = m_kneePos + m_endPos /*m_legFrame.transform.TransformDirection()*/; // PID test if (m_testPIDLower != null && m_testPIDUpper != null) { Quaternion localUpper = /*Quaternion.Inverse(m_legFrame.transform.rotation) **/ m_upperLeg.rotation; Quaternion localLower = Quaternion.Inverse(m_upperLeg.rotation) * m_lowerLeg.rotation; Quaternion localGoalUpper = Quaternion.AngleAxis(Mathf.Rad2Deg * (upperLegAngle + Mathf.PI * 0.5f), Vector3.left); Quaternion localGoalLower = Quaternion.AngleAxis(Mathf.Rad2Deg * (lowerLegAngle /* - Mathf.PI*0.5f*/), Vector3.left); m_testPIDUpper.drive(localUpper, localGoalUpper, Time.deltaTime); m_testPIDLower.drive(localLower, localGoalLower, Time.deltaTime); } } else { m_kneePos += upperLegLocalPos; m_endPos += m_kneePos; } Vector3 offset = Vector3.zero; int idx = (int)m_legType; if (m_legFrame != null) { offset = new Vector3(m_legFrame.m_footTarget[idx].x, 0.0f /*m_legFrame.transform.position.y*/, m_startPos.z + m_legFrame.getOptimalProgress() /*-m_legFrame.getReferenceLiftPos(idx).z/* + m_legFrame.transform.position*/); } //offset = new Vector3(m_legFrame.getReferenceFootPos(idx).x, m_legFrame.transform.position.y, m_legFrame.getReferenceFootPos(idx).z); else if (m_foot != null) { offset = new Vector3(m_upperLeg.position.x, 0.0f, m_upperLeg.position.z) /* + m_legFrame.transform.position*/; } m_hipPos = offset + upperLegLocalPos; m_kneePosW = offset + m_kneePos; Debug.DrawLine(offset + upperLegLocalPos, m_kneePosW, Color.red); Debug.DrawLine(m_kneePosW, offset + m_endPos, Color.blue); if (m_dbgMesh) { m_dbgMesh.rotation = m_legFrame.transform.rotation * Quaternion.AngleAxis(Mathf.Rad2Deg * (upperLegAngle + Mathf.PI * 0.5f), -m_legFrame.transform.right); m_dbgMesh.position = upperLegLocalPos; } }
// Drives the PD-controller and retrieves the 3-axis torque // vector that will be used as the desired torque for which the // stance legs tries to accomplish. private Vector3 getPDTorque(Quaternion p_desiredOrientation) { Vector3 torque = m_desiredLFTorquePD.drive(transform.rotation, p_desiredOrientation, Time.deltaTime); return(torque); }