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); }