public IEnumerator IKAuto(List <float> angles, List <LinkParam> linkParams, FKManager fk)
{
int culcCount = 0;
double[,] q = angles.ToVertical();
double[,] prev_e = new double[6, 1];
while (true /*culcCount < 100000*/)
{
var HTMs = HomogeneourCoordinate.GetHTMs(linkParams, angles);
var J = fk.GetJacobian(HTMs).Matrix;
var J_T = J.Transpose();
var EndHTM = HTMs[HTMs.Count() - 1];
var e_q = Error(fk.GetEndPosition(EndHTM), EndHTM.RotationMatrix());
for (int i = 0; i < 3; i++)
{
W_e[i, i] = w_e_pos;
}
for (int i = 3; i < 6; i++)
{
W_e[i, i] = w_e_rot;
}
var Jt_W_J_plus_Wn__inv = (J_T.Times(W_e).Times(J).Plus(W_n(e_q, W_e))).inverseMatrix();
var delta_q = Jt_W_J_plus_Wn__inv.Times(J_T).Times(W_e).Times(e_q);
q = q.Plus(delta_q);
for (int i = 0; i < q.GetLength(0); i++)
{
angles[i] = (float)q[i, 0];
}
prev_e = e_q;
culcCount++;
if (culcCount % 100 == 0)
{
yield return(null);
}
}
print("loop num: " + culcCount);
yield break;
}
IEnumerator TargetMove(Vector3 targetPos, List <List <float> > midsTargetAngles)
{
var startAngle = robot.GetAngles();
var deltas = new List <float>();
deltas.Add(GetMagnitude(startAngle, midsTargetAngles[0]));
for (var i = 1; i < midsTargetAngles.Count(); ++i)
{
deltas.Add(GetMagnitude(midsTargetAngles[i - 1], midsTargetAngles[i]));
}
var allDelta = deltas.Sum();
var phasies = deltas.Select(d => d / allDelta).ToList();
var moveTime = allDelta / aveSpeed;
var currentTime = 0.0f;
int moveloop = 0;
print("moving");
int index = 0;
FKManager.Foreach(linkParams, midsTargetAngles[index], fk);
float offset = 0;
float progress = 0;
while (!IsCloseEnough(robot.CurrentEndPosition(), targetPos) && moveloop < maxMoveLoop && currentTime < moveTime)
{
if (progress >= 1)
{
offset += phasies[index];
++index;
startAngle = robot.GetAngles();
FKManager.Foreach(linkParams, midsTargetAngles[index], fk);
}
robot.SetAngle(AnglesDeltaByPhase(robot.GetAngles(), startAngle, midsTargetAngles[index], offset, phasies[index], out progress, currentTime, moveTime));
moveloop++;
currentTime += Time.deltaTime;
yield return(null);
}
}
IEnumerator AngleMove(List <float> targetAngle)
{
int moveloop = 0;
var moveTime = GetMagnitude(robot.GetAngles(), targetAngle) / aveSpeed;
var currentTime = 0.0f;
FKManager.Foreach(linkParams, targetAngle, fk);
print("moving");
var startAngle = robot.GetAngles();
while (robot.GetAngles().Where(a => a != 0).Any() && moveloop < maxMoveLoop && currentTime < moveTime)
{
robot.SetAngle(AnglesDelta(robot.GetAngles(), startAngle, targetAngle, currentTime, moveTime));
moveloop++;
currentTime += Time.deltaTime;
yield return(null);
}
}
IEnumerator TargetMove(Vector3 targetPos, List <float> targetAngle)
{
var startAngle = robot.GetAngles();
var moveTime = GetMagnitude(robot.GetAngles(), targetAngle) / aveSpeed;
var currentTime = 0.0f;
int moveloop = 0;
FKManager.Foreach(linkParams, targetAngle, fk);
print("moving");
while (!IsCloseEnough(robot.CurrentEndPosition(), targetPos) && moveloop < maxMoveLoop && currentTime < moveTime)
{
robot.SetAngle(AnglesDelta(robot.GetAngles(), startAngle, targetAngle, currentTime, moveTime));
moveloop++;
currentTime += Time.deltaTime;
yield return(null);
}
}
public static Vector3 GetEndPosition(List <LinkParam> linkParams, List <float> angles, FKManager fk)
{
return(HomogeneourCoordinate.GetHTMs(linkParams, angles)[~1] * fk.hE);
}
public static void Foreach(List <LinkParam> linkParams, List <float> angles, FKManager fk)
{
fk.Foreach(HomogeneourCoordinate.GetHTMs(linkParams, angles));
}
IEnumerator TaskSequence()
{
List <float> targetAngle = new List <float>();
fk = robot.GetFK();
linkParams = robot.GetLinkParams();
var taskCount = 0;
int moveloop;
foreach (var task in taskObjects)
{
robot.SetInControl(true);
var midAngleList = new List <List <float> >();
var taskAbove = task.position + new Vector3(0, 0.1f, 0);
targetAngle = robot.GetAngles();
if (taskCount == 0)
{
yield return(StartCoroutine(robot.CulcIK(targetAngle, taskAbove, Quaternion.Euler(-90, -90, 0))));
yield return(StartCoroutine(TargetMove(taskAbove, targetAngle)));
}
else
{
midAngleList = new List <List <float> >();
foreach (var p in midPoints)
{
yield return(StartCoroutine(robot.CulcIK(targetAngle, p.position, Quaternion.Euler(-90, 180, 0))));
midAngleList.Add(new List <float>(targetAngle));
}
midAngleList.Reverse();
yield return(StartCoroutine(robot.CulcIK(targetAngle, taskAbove, Quaternion.Euler(-90, -90, 0))));
midAngleList.Add(new List <float>(targetAngle));
yield return(StartCoroutine(TargetMove(taskAbove, midAngleList)));
}
yield return(StartCoroutine(robot.CulcIK(targetAngle, task.position + new Vector3(0, 0.01f, 0), Quaternion.Euler(-90, -90, 0))));
yield return(StartCoroutine(TargetMove(task.position + new Vector3(0, 0.01f, 0), targetAngle)));
moveloop = 0;
print("gripping");
float gripWidth = 0;
while (gripWidth < 0.28f)
{
gripWidth += pg;
grip.SetWidth(gripWidth);
moveloop++;
yield return(null);
}
yield return(StartCoroutine(robot.CulcIK(targetAngle, taskAbove, Quaternion.Euler(-90, -90, 0))));
yield return(StartCoroutine(TargetMove(taskAbove, targetAngle)));
midAngleList = new List <List <float> >();
foreach (var p in midPoints)
{
yield return(StartCoroutine(robot.CulcIK(targetAngle, p.position, Quaternion.Euler(-90, 180, 0))));
midAngleList.Add(new List <float>(targetAngle));
}
yield return(StartCoroutine(robot.CulcIK(targetAngle, goalArea.transform.position, Quaternion.Euler(-90, 90, 0))));
midAngleList.Add(new List <float>(targetAngle));
yield return(StartCoroutine(TargetMove(goalArea.position, midAngleList)));
//FKManager.Foreach(linkParams, targetAngle, fk);
//yield return StartCoroutine(TargetMove(goalArea.position, targetAngle));
moveloop = 0;
print("gripping");
while (gripWidth >= 0)
{
gripWidth -= pg;
grip.SetWidth(gripWidth);
moveloop++;
yield return(null);
}
++taskCount;
}
targetAngle = targetAngle.Select(a => 0f).ToList();
yield return(StartCoroutine(AngleMove(targetAngle)));
robot.SetInControl(false);
}
