/* Simulates the forward kinematics,
* given a solution. */
public PositionRotation ForwardKinematics(float[] Solution)
{
Vector3Class prevPoint = new Vector3Class(Joints[0].transform.position);
// Takes object initial rotation into account
QuaternionClass rotation = new QuaternionClass();
rotation.SetValues(transform.rotation);
for (int i = 1; i < Joints.Length; i++)
{
QuaternionClass angleAxis = new QuaternionClass();
angleAxis.convertFromAxisAngle(Joints[i - 1].Axis, Solution[i - 1]);
rotation *= angleAxis;
Vector3Class nextPoint = prevPoint + rotation.multiplyVec3(rotation, Joints[i].StartOffset);
if (DebugDraw)
{
Debug.DrawLine(prevPoint.GetValues(), nextPoint.GetValues(), Color.blue);
}
prevPoint = nextPoint;
}
// The end of the effector
return(new PositionRotation(prevPoint, rotation));
}
void DrawLines()
{
Vector3Class angularVec = new Vector3Class(curAngularVelX, 0, curAngularVelZ);
angularVec = angularVec.CrossProduct(angularVec, stringVector);
Debug.DrawLine(pos2.position, angularVec.GetValues(), Color.green);
}
public void ResetPendulum(Vector3Class pos)
{
//actualiza la posicion de pos2 para setear la bola segn el target de la escena
pos2.position = pos.GetValues();
List <CilinderBody> tempRopeSegments = new List <CilinderBody>();
//recibir las posiciones de los 2 extremos de la cuerda
Vector3Class initPos = new Vector3Class(pos1.transform.position);
Vector3Class endPos = new Vector3Class(pos2.transform.position);
//generador de los puntos de la cuerda, en resumen hace un vector entre pos1,pos2
Vector3Class stringVector = endPos - initPos;
stringVector = stringVector.Normalize(stringVector);
List <Vector3Class> ropePositions = new List <Vector3Class>();
//determina el punto inicial de todos los segmentos con
//"largo segmento * posicion en lista * direccion normalizadapos1pos2 + pos1"
for (int i = 0; i < stringPartitions; i++)
{
Vector3Class partPos = initPos;
partPos += stringVector * i * stringPartLength;
ropePositions.Add(partPos);
}
//se añaden todos los segmentos con la posicion deseada
//se introducen los elementos al reves para facilitar algunos calculos
for (int i = ropePositions.Count - 1; i >= 0; i--)
{
tempRopeSegments.Add(new CilinderBody(ropePositions[i]));
}
ropeSegments = tempRopeSegments;
//actualiza posicion del objeto bola
pos2.position = ropeSegments[0].pos.GetValues();
}
// Update is called once per frame
void Update()
{
if (statusSimulation != States.done)
{
timer += Time.deltaTime;
}
if (startSimulation /*Input.GetKeyDown("space")*/ && statusSimulation == States.done)
{
startSimulation = false;
statusSimulation = States.init;
roboticHand.NewDestination(pendulum.GetBall());
for (int i = 0; i < 5; i++)
{
fingers[i].GetComponent <FingerManager>().CloseHand();
}
}
if (statusSimulation == States.init && timer > timeToStartSimulation) //empieza movimiento pendulo con la mano
{
pendulum.SetMove(true);
roboticHand.perfFollow = true;
timer = 0;
statusSimulation = States.simulationHandFollow;
}
if (statusSimulation == States.simulationHandFollow && timer > timeToStopHandFollow - 0.2f && !openedHands) //la mano empieza a soltar la bola
{
openedHands = true;
for (int i = 0; i < 5; i++)
{
fingers[i].GetComponent <FingerManager>().OpenHand();
}
}
if (statusSimulation == States.simulationHandFollow && timer > timeToStopHandFollow) //la mano deja de seguir al pendulo
{
roboticHand.perfFollow = false;
target.position = defHandPos.position;
roboticHand.NewDestination(target);
statusSimulation = States.simulationNoHand;
}
if (statusSimulation == States.simulationNoHand && timer > timeToStopSimulation) //la mano se dirige a la posicion predict
{
bool reachable = false;
Vector3Class tPos = pendulum.CalculateFuturePosition(timeToPredictStopPosition);
Vector3Class baseHandPos = new Vector3Class(roboticHand.transform.position);
while (!reachable)
{
tPos = pendulum.CalculateFuturePosition(timeToPredictStopPosition);
if (tPos.Distance(tPos, baseHandPos) < 2.2f)
{
reachable = true;
}
else
{
timeToPredictStopPosition += 0.5f;
}
}
target.position = tPos.GetValues();
roboticHand.NewDestination(target);
timer = 0;
statusSimulation = States.closeHand;
}
if (statusSimulation == States.closeHand && timer > timeToPredictStopPosition - 0.3f) //se empiezan a cerrar las manos
{
statusSimulation = States.stopPendulum;
for (int i = 0; i < 5; i++)
{
fingers[i].GetComponent <FingerManager>().CloseHand();
}
}
if (statusSimulation == States.stopPendulum && timer > timeToPredictStopPosition) //se para el pendulo
{
pendulum.SetMove(false);
canvas.AllowInputP(true);
timeToPredictStopPosition = initStopTime;
statusSimulation = States.done;
openedHands = false;
}
}
// Update is called once per frame
void Update()
{
target = new Vector3Class(Destination.transform.position);
if (this.tag == "offset")
{
//Algunos ajustes para que la mano se posicione relativamente del target con mas realismo
Vector3Class P = new Vector3Class(BaseJoint.transform.position); //Joints[0].transform.position ??
Vector3Class Q = new Vector3Class(Destination.transform.position);
//Normalizar distancia de Q(Pendulo) a P(Effector) y multiplicar por el radio de la bola del pendulo + offset adicional de ajuste para obtener el offset del target total:
Vector3Class offsetTarget = Q - P;
//Debug.Log("x" + offsetTarget.x + "y" + offsetTarget.y + "z" + offsetTarget.z);
float totalOffset = 0.3f; //<-Poner aqui un offset adicional... En realidad sería la mitad del grosor de la mano si el endefector HAND estuviese justo a la mitad.
offsetTarget = offsetTarget.Normalize(offsetTarget) * (offsetTarget.Size() - totalOffset);
offsetTarget += P; //Actualiza posicion del objetivo
offsetTarget.y = target.y;
target = offsetTarget;
}
//ApproachTarget(target);
//ForwardKinematics(Solution);
if (perfFollow && tag == "arm")
{
int counterWhile = 0;
while (counterWhile < 20)
{
counterWhile++;
if (ErrorFunction(target, Solution) > StopThreshold)
{
ApproachTarget(target);
for (int i = 0; i < Joints.Length; i++)
{
Joints[i].MoveArm(Solution[i]);
}
}
else
{
break;
}
}
}
else
{
if (ErrorFunction(target, Solution) > StopThreshold)
{
ApproachTarget(target);
for (int i = 0; i < Joints.Length; i++)
{
Joints[i].MoveArm(Solution[i]);
}
}
}
if (DebugDraw)
{
Debug.DrawLine(Effector.transform.position, target.GetValues(), Color.green);
Debug.DrawLine(Destination.transform.position, target.GetValues(), new Color(0, 0.5f, 0));
}
}
public void NewPendulumAngle(Vector3Class angle)
{
transform.localEulerAngles = angle.GetValues();
}
