| public drive ( Vector2 p_error, float p_dt ) : Vector2 | ||
| p_error | Vector2 | |
| p_dt | float | |
| Результат | Vector2 | |
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);
}