public void setBodySpringStuff(CWRigidBody body, float spring, float damper, bool isFirst = true)
{
if (isFirst && body.getParentJoint() != null)
{
//Debug.Log(" ===== body === "+body.getParentJoint().jName);
body.getParentJoint().setSpring(spring);
body.getParentJoint().setDamper(damper);
if (body.name.Contains("LowerArm") || body.name.Contains("LowerLeg"))
{
body.getParentJoint().getParent().getParentJoint().setSpring(spring);
body.getParentJoint().getParent().getParentJoint().setDamper(damper);
}
}
int count = body.getChildJointCount();
for (int i = 0; i < count; i++)
{
CWJoint joint = body.getChildJoint(i);
joint.setSpring(spring);
joint.setDamper(damper);
//Debug.Log(" ===== child body === "+joint.jName);
if (joint.getChild().getChildJointCount() > 0)
{
setBodySpringStuff(joint.getChild(), spring, damper, false);
}
}
}
/**
* This method is used to get the relative angular velocities of the parent and child bodies of joint i,
* expressed in parent's local coordinates.
* We'll assume that i is in the range 0 - joints.size()-1!!!
*/
public Vector3 getRelativeAngularVelocity(int i)
{
CWJoint joint = this.joints[i] as CWJoint;
Quaternion parentOri = joint.getParent().getOrientation();
return(Quaternion.Inverse(parentOri) * (joint.getChild().getAngularVelocity() - joint.getParent().getAngularVelocity()));
}
/**
* This method is used to populate the relative orientation of the parent and child bodies of joint i.
*/
public Quaternion getRelativeOrientation(int i)
{
CWJoint joint = this.joints[i] as CWJoint;
Quaternion parentOri = joint.getParent().getOrientation();
Quaternion childOri = joint.getChild().getOrientation();
return(Quaternion.Inverse(parentOri) * childOri);
}
/**
* this method is used to return the index of the joint (whose name is passed as a parameter) in the articulated figure hierarchy.
*/
public int getJointIndex(CWJoint joint)
{
for (int i = 0; i < joints.Count; i++)
{
if (joints[i] == joint)
{
return(i);
}
}
return(-1);
}
/**
* this method is used to return the index of the joint (whose name is passed as a parameter) in the articulated figure hierarchy.
*/
public int getJointIndex(string jName)
{
for (int i = 0; i < joints.Count; i++)
{
CWJoint joint = joints[i] as CWJoint;
if (joint.jName == jName)
{
return(i);
}
}
return(-1);
}
/**
* this method is used to return a reference to the joint whose name is passed as a parameter, or NULL
* if it is not found.
*/
public CWJoint getJointByName(string jName)
{
for (int i = 0; i < joints.Count; i++)
{
CWJoint joint = joints[i] as CWJoint;
if (joint.jName == jName)
{
return(joint);
}
}
return(null);
}
/**
* All quantities that are passed in as parameters here need to be expressed in the same "global" coordinate frame, unless otherwise noted:
* - p1: this is the location of the origin of the parent link
* - p2: this is the target location of the end effector on the child link
* - n: this is the default normal to the rotation plane (it will get modified as little as possible to account for the target location)
*
* - vParent: vector from parent origin to child origin, expressed in parent coordinates
* - nParent: this is the rotation axis, expressed in parent coordinates. The relative orientation between child and parent will be about this axis
* - vChild: vector from child origin, to position of the end effector, expressed in child coordinates
*
* Output:
* - qP: relative orientation of parent, relative to "global" coordinate frame
* - qC: relative orientation of child, relative to the parent coordinate frame
*
*
* NOTE: for now, the vector vChild is pretty much ignored. Only its length is taken into account. The axis that the child is lined up around is the same as the direction
* of the vector from the parent's origin to the child's origin (when there is zero relative orientation between the two, the axis has the same coordinates in both
* child and parent frame).
*/
public static void getIKOrientations(Controller con, CWJoint pJoint, CWRigidBody gParent, Vector3 p1, Vector3 p2, Vector3 n, Vector3 vParent, Vector3 nParent, Vector3 vChild, ref Quaternion qP, ref Quaternion qC)
{
//modify n so that it's perpendicular to the vector (p1, p2), and still as close as possible to n
Vector3 line = p2 - p1;
//Debug.DrawLine(gParent.getWorldCoordinatesPoint(p1), gParent.getWorldCoordinatesPoint(p2), Color.red);
Vector3 temp = Vector3.Cross(n, line);
//Debug.DrawLine(gParent.getWorldCoordinatesPoint(p1), gParent.getWorldCoordinatesPoint(p1) + 5*temp, Color.red);
Vector3 nG = Vector3.Cross(line, temp);
nG.Normalize();
//Debug.DrawLine(gParent.getWorldCoordinatesPoint(p1), gParent.getWorldCoordinatesPoint(p1) + gParent.getWorldCoordinatesVector(5*nG), Color.red);
//now compute the location of the child origin, in "global" coordinates
Vector3 solvedJointPosW = TwoLinkIK.solve(p1, p2, nG, vParent.magnitude, vChild.magnitude);
Vector3 vParentG = solvedJointPosW - p1;
Vector3 vChildG = p2 - solvedJointPosW;
//now we need to solve for the orientations of the parent and child
//if the parent has 0 relative orientation to the grandparent (default), then the grandparent's orientation is also the parent's
//Debug.DrawLine(gParent.getWorldCoordinatesPoint(solvedJointPosW), gParent.getWorldCoordinatesPoint(solvedJointPosW+nG),Color.red);
//Debug.DrawLine(gParent.getWorldCoordinatesPoint(p1), gParent.getWorldCoordinatesPoint(solvedJointPosW),Color.red);
//Debug.DrawLine(gParent.getWorldCoordinatesPoint(p2), gParent.getWorldCoordinatesPoint(solvedJointPosW),Color.red);
if (con.getCharacter().getRoot() == gParent)
{
qP = TwoLinkIK.getParentLegOrientation(con, vParentG);
}
else
{
qP = TwoLinkIK.getParentArmOrientation(con, pJoint, gParent, vParentG, vChildG);
}
float childAngle = TwoLinkIK.getChildRotationAngle(vParentG, vChildG, nG);
if (Vector3.Dot(nG, nParent) < 0)
{
childAngle = -childAngle;
}
qC = Quaternion.AngleAxis(childAngle, Vector3.right);
//Debug.Log("aaaaaa "+qP.ToString("F4"));
//Debug.Log("bbbbbb "+qC.ToString("F4"));
}
public void addChildJoint(CWJoint j)
{
cJoints.Add(j);
}
public void setParentJoint(CWJoint j)
{
pJoint = j;
}
/**
* Adds a joint to the figure
* This is an empty function as the joints are not tracked
* by the ArticulatedFigure.
* This makes it possible to disown Python of the joint pointer
* so that it doesn't garbage collect it.
* The real place where Python should be disowned is when
* Joint.setParent() is called since the parent is responsible
* for deleting the joint. However, I don't know how to force
* python to disown an object when a method is called.
*/
public void addJoint(CWJoint joint_disown)
{
joints.Add(joint_disown);
}
/**
* This method determines the orientation for the parent link, relative to some other coordinate ("global") frame.
* Two vectors (one that goes from the parent origin to the child origin v, as well as a normal vector n) are known,
* expressed both in the global frame and the parent frame. Using this information, we can figure out the relative orientation
* between the parent frame and the global frame.
*
* Input:
* vGlobal - parent's v expressed in grandparent coordinates
* nGlobal - this is the rotation axis that is used for the relative rotation between the child and the parent joint, expressed in
* grandparent coordinates
* vLocal - parent's v expressed in the parent's local coordinates
* nLocal - this is the rotation axis that is used for the relative rotation between the child and the parent joint, expressed in
* parent's local coordinates
* Output:
* q - the relative orientation between the parent and the grandparent (i.e. transforms vectors from parent coordinates to grandparent coordinates).
*/
public static Quaternion getParentArmOrientation(Controller con, CWJoint pJoint, CWRigidBody gParent, Vector3 vParent, Vector3 vChild)
{
Quaternion q = Quaternion.identity;
Vector3 up = Vector3.zero, left = Vector3.zero, front = Vector3.zero;
Vector3 tPos = vParent + vChild;
float tangle = Vector3.Angle(tPos, con.getCharacter().getLocalUpAxis());
float vangle = Vector3.Angle(vParent, vChild);
//Debug.Log(tangle+ " aaaaaaa "+vangle + " aaaa "+tPos[con.getLocalFrontAxisID()]+" aaa "+tPos[con.getLocalLeftAxisID()]);
if (tangle <= 90)
{
if (tPos[con.getLocalFrontAxisID()] < 0 && vangle > 80)
{
up = -1 * vParent.normalized;
left = Vector3.Cross(vParent, vChild).normalized;
front = Vector3.Cross(left, up).normalized;
up = gParent.getOrientation() * up;
front = gParent.getOrientation() * front;
q = gParent.getOrientation() * Quaternion.LookRotation(Vector3.left, Vector3.down);
}
else
{
up = vParent.normalized;
left = Vector3.Cross(vParent, vChild).normalized;
front = Vector3.Cross(left, up).normalized;
up = gParent.getOrientation() * up;
front = gParent.getOrientation() * front;
q = gParent.getOrientation() * Quaternion.LookRotation(Vector3.right, Vector3.up);
}
}
else
{
if (tPos[con.getLocalFrontAxisID()] > 0 && vangle > 30)
{
up = vParent.normalized;
left = Vector3.Cross(vParent, vChild).normalized;
front = Vector3.Cross(left, up).normalized;
up = gParent.getOrientation() * up;
front = gParent.getOrientation() * front;
q = gParent.getOrientation() * Quaternion.LookRotation(Vector3.right, Vector3.up);
}
else
{
up = -1 * vParent.normalized;
left = Vector3.Cross(vParent, vChild).normalized;
front = Vector3.Cross(left, up).normalized;
up = gParent.getOrientation() * up;
front = gParent.getOrientation() * front;
q = gParent.getOrientation() * Quaternion.LookRotation(Vector3.left, Vector3.down);
}
}
return(Quaternion.Inverse(q) * Quaternion.LookRotation(up, front));
}
