void Start() { _robotArm = GetComponent <RobotArm>(); _ik = GetComponent <RobotArmIK>(); _targetAngles = new TargetPositions(); _targetPositions = new List <PositionAndEndeffectorAngle>(); /* * // look up/down * _targetPositions.Add(new PositionAndEndeffectorAngle { * Position = new Vector3(134f, 60f, 0f), * EndeffectorAngle = Mathf.PI / 2f + 0.4f, * ElbowUp = true * }); * * _targetPositions.Add(new PositionAndEndeffectorAngle { * Position = new Vector3(144f, 94f, 0f), * EndeffectorAngle = Mathf.PI / 2f, * ElbowUp = true * }); * * _targetPositions.Add(new PositionAndEndeffectorAngle { * Position = new Vector3(144f, 94f, 0f), * EndeffectorAngle = Mathf.PI / 2f - 0.4f, * ElbowUp = true * }); * * _targetPositions.Add(new PositionAndEndeffectorAngle { * Position = new Vector3(144f, 94f, 0f), * EndeffectorAngle = Mathf.PI / 2f + 0.4f, * ElbowUp = true * }); */ // drill left _targetPositions.Add(new PositionAndEndeffectorAngle { Position = new Vector3(124f, 74f, 0f), EndeffectorAngle = Mathf.PI / 2f, ElbowUp = true }); _targetPositions.Add(new PositionAndEndeffectorAngle { Position = new Vector3(144f, 74f, 0f), EndeffectorAngle = Mathf.PI / 2f, ElbowUp = true }); _targetPositions.Add(new PositionAndEndeffectorAngle { Position = new Vector3(124f, 74f, 0f), EndeffectorAngle = Mathf.PI / 2f, ElbowUp = true }); }
// - - - - - - - - - - - - - - - // - - - - - RUN IK - - - - - - // - - - - - - - - - - - - - - - public TargetPositions RunIK(float x, float y, float ohm, bool elbowUp) { Vector2 _P_A, _P_B, _P_C; float _g, _f; float _phi, _c, _d, _e; float _lambda1, _lambda2, _lambda3; float _gamma, _alpha, _beta; // Note how there's no z-coordinate. // 1. Set up P_endeffector and the angle "ohm" Vector2 _P_endeffector = new Vector2(elbowUp ? x : -x, y); float _ohm = ohm; // 2. Calculate point A Vector2 P_origin = new Vector2(0.0f, 0.0f); _P_A = new Vector2( P_origin.x, P_origin.y + _link1); // 3. Calculate point B _g = Mathf.Cos(_ohm) * _link4; _f = Mathf.Sin(_ohm) * _link4; _P_B = new Vector2( _P_endeffector.x - _f, _P_endeffector.y + _g); // 4. Calculate angle phi _d = _P_B.x - _P_A.x; _e = _P_B.y - _P_A.y; _phi = Mathf.Atan2(_e, _d); // 5. Calculate the length of side c _c = Mathf.Sqrt(_d * _d + _e * _e); // 6. Calculate angles gamma, alpha and beta _gamma = Mathf.Acos((_link2 * _link2 + _link3 * _link3 - _c * _c) / (2 * _link2 * _link3)); _alpha = Mathf.Acos((_link2 * _link2 + _c * _c - _link3 * _link3) / (2 * _link2 * _c)); _beta = Mathf.PI - _gamma - _alpha; // 7. Calculate angles lambda1, lambda2 and lambda3 in the case of "elbow up." _lambda1 = Mathf.PI / 2 - (_phi + _alpha); _lambda2 = Mathf.PI - _gamma; _lambda3 = Mathf.PI - ((Mathf.PI / 2 - _phi) + _beta) - _ohm; // 8. Calculate angles lambda1, lambda2 and lambda3 in the case of "elbow down." if (!elbowUp) { _lambda1 = -_lambda1; _lambda2 = -_lambda2; _lambda3 = -_lambda3; } var targets = new TargetPositions(); targets.Link1 = _lambda1; // 0 targets.Link2 = _lambda2; // 1 targets.Link3 = _lambda3; // 2 /* * Serial.print("lambda1: "); * Serial.println(_lambda1); * Serial.print("lambda2: "); * Serial.println(_lambda2); * Serial.print("lambda3: "); * Serial.println(_lambda3); */ if (_lambda1 != _lambda1 || _lambda2 != _lambda2 || _lambda3 != _lambda3) { //Debug.Log("NaN error!"); return(null); } return(targets); }