public Configuration(Vector3 position, Quaternion rotation, Finger[] fingerList)
{
transform = new PositionRotation(position, rotation);
FingerList = new List <PositionRotation[]>();
foreach (var finger in fingerList)
{
var jointlist = new PositionRotation[finger.JointNum];
jointlist = finger.GetJointsPose();
FingerList.Add(jointlist);
}
}
public Configuration(Wrist Hand)
{
transform = new PositionRotation(Hand.transform.position, Hand.transform.rotation);
FingerList = new List <PositionRotation[]>();
foreach (var finger in Hand.FingerList)
{
var jointlist = new PositionRotation[finger.JointNum];
jointlist = finger.GetJointsPose();
FingerList.Add(jointlist);
}
}
public Configuration(Vector3 position, Quaternion rotation, List <PositionRotation[]> fingerList)
{
transform = new PositionRotation(position, rotation);
FingerList = new List <PositionRotation[]>();
foreach (var finger in fingerList)
{
var jointlist = new PositionRotation[finger.Length];
for (int i = 0; i < finger.Length; i++)
{
jointlist[i] = new PositionRotation(finger[i], finger[i]);
}
FingerList.Add(jointlist);
}
}
void SampleHandPosition()
{
GripPoints newGrip = new GripPoints(PossibleHoldPoints[Mathf.RoundToInt(Random.Range(0, PossibleHoldPoints.Count - 1))]);
PositionRotation[] points = new PositionRotation[HandObject.FingerList.Length];
for (int i = 0; i < HandObject.FingerList.Length; i++)
{
var pads = HandObject.FingerList[i].GetComponentsInChildren <FingerPad>();
points[i] = pads[pads.Length - 1].JointPositionFromPad(newGrip.GetGrip()[i]);
}
// From touch points back calculate finger poses;
//
}
public Configuration(Configuration c)
{
transform = new PositionRotation(c.transform, c.transform);
FingerList = new List <PositionRotation[]>();
foreach (var finger in c.FingerList)
{
var jointlist = new PositionRotation[finger.Length];
for (int i = 0; i < finger.Length; i++)
{
jointlist[i] = new PositionRotation(finger[i], finger[i]);
}
FingerList.Add(jointlist);
}
if (c.Joints != null)
{
AddJointAngles(c.Joints);
}
}
/* Calculates the gradient for the invetse kinematic.
* It simulates the forward kinematics the i-th joint,
* by moving it +delta and -delta.
* It then sees which one gets closer to the target.
* It returns the gradient (suggested changes for the i-th joint)
* to approach the target. In range (-1,+1)
*/
public float CalculateGradient(PositionRotation target, float[] m_Solution, int i, float delta)
{
// Saves the angle,
// it will be restored later
float solutionAngle = m_Solution[i];
// Gradient : [F(x+h) - F(x)] / h
// Update : Solution -= LearningRate * Gradient
float f_x = ErrorFunction(target, m_Solution);
m_Solution[i] += delta;
float f_x_plus_h = ErrorFunction(target, m_Solution);
float gradient = (f_x_plus_h - f_x) / delta;
// Restores
m_Solution[i] = solutionAngle;
return(gradient);
}
public PositionRotation[] FKSimBuild(PositionRotation[] prevJoints, float[] newAngles)
{
var newJointSim = new PositionRotation[prevJoints.Length];
Vector3 prevPoint = prevJoints[0];
//Quaternion rotation = Quaternion.identity;
// Takes object initial rotation into account
Quaternion rotation = prevJoints[0];
newJointSim[0] = prevJoints[0];
for (int i = 1; i < Joints.Length; i++)
{
// Rotates around a new axis
rotation *= Quaternion.AngleAxis(newAngles[i - 1], Joints[i - 1].Axis);
Vector3 nextPoint = prevPoint + rotation * Joints[i].StartOffset;
prevPoint = nextPoint;
newJointSim[i] = new PositionRotation(prevPoint, rotation);
}
return(newJointSim);
}
public List <float[]> TestPath(List <float[]> SolutionSteps, PositionRotation[] joints, PositionRotation endPoint)
{
List <float[]> m_solutionSteps = new List <float[]>();
for (int i = 0; i < SolutionSteps.Count; i++)
{
float[] steps = new float[SolutionSteps[i].Length];
for (int j = 0; j < SolutionSteps[i].Length; j++)
{
steps[j] = SolutionSteps[i][j];
}
m_solutionSteps.Add(steps);
}
float[] m_Solution = new float[Joints.Length];
for (int j = 0; j < SolutionSteps[m_solutionSteps.Count - 1].Length; j++)
{
m_Solution[j] = m_solutionSteps[m_solutionSteps.Count - 1][j];
}
JointSim = new PositionRotation[Joints.Length];
string debugPrint = "";
for (var i = 0; i < joints.Length; i++)
{
JointSim[i] = joints[i];
debugPrint += JointSim[i] + "\n";
}
//JointSim = joints;
//Debug.Log(debugPrint);
//Debug.Log("Step point is: " + endPoint);
var target = endPoint;
for (var i = 0; i < MaximumLoop; i++)
{
if (ErrorFunction(target, m_Solution) > StopThreshold)
{
var newSoln = ApproachTarget(target, m_Solution);
float[] steps = new float[m_Solution.Length];
for (int j = 0; j < m_Solution.Length; j++)
{
steps[j] = m_Solution[j];
}
m_solutionSteps.Add(steps);
/* Debug Print Array Start */
debugPrint = "";
foreach (var angle in m_Solution)
{
debugPrint += angle.ToString() + "\n";
}
//Debug.Log("Intermediate soln " + i + ":\n" + debugPrint);
//Debug.Log("Solution list is " + m_solutionSteps.Count + " steps long");
/* Debug Print Array End */
UpdateJointPosition(m_Solution);
/* Debug Print Array Start */
debugPrint = "";
foreach (var angle in JointSim)
{
debugPrint += angle.ToString() + "\n";
}
//Debug.Log("Joints at " + i + " are: " + debugPrint);
/* Debug Print Array End */
}
else
{
return(m_solutionSteps);
}
}
//Debug.Log("No path found");
return(null);
}
void Awake()
{
ZeroEuler = transform.localEulerAngles;
StartOffset = transform.localPosition;
HomePose = new PositionRotation(transform.position, transform.rotation);
}
