void FindSafePath()
{
bool hasPath = false;
//int shortestSet = ;
MovePath = new Solution[ArmSolutions.Length];
while (!hasPath && ArmSolutions[0].Count > 0)
{
SolutionList[] shortFirst = new SolutionList[ArmSolutions.Length];
shortFirst[0].Add(ArmSolutions[0].PopShortest());
for (int i = 1; i < ArmSolutions.Length; i++)
{
shortFirst[i] = ArmSolutions[i].GetCollisionFreeList(shortFirst);
}
hasPath = true;
for (int i = 0; i < ArmSolutions.Length; i++)
{
MovePath[i] = shortFirst[i].PopShortest();
if (MovePath[i] == null)
{
hasPath = false;
}
}
}
}
public void SetNewObject(GraspRegion target)
{
if (Destination != null && Destination != target)
{
Destination.Disconnect();
Destination = null;
}
if (Destination == null)
{
Destination = target;
ArmTree = new Tree();
GoalTree = new Tree();
SolutionPathList = new SolutionList();
//Destination.Connect(this);
}
}
// Use this for initialization
void Start()
{
// Initialize Trees and solutions
ArmTree = new Tree();
GoalTree = new Tree();
SolutionPathList = new SolutionList();
ArmColliderList = GetComponentsInChildren <Collider>();
armSolver = GetComponent <ArmSolver>();
// Initialize distance tolerances
Delta = Toolbox.Instance.GetConnectionDistance();
if (HandObject == null)
{
HandObject = GetComponentInChildren <Wrist>();
}
doSearch = true; // Placeholder until multiarm is built
}
// Use this for initialization
void Start()
{
// Initialize Trees and solutions
ArmTree = new Tree();
GoalTree = new Tree();
SolutionPathList = new SolutionList();
// Initialize distance tolerances
Delta = Toolbox.Instance.GetConnectionDistance();
Joints = GetComponentsInChildren <RobotJoint>();
ikSolver = GetComponent <IKSolver>();
motionController = GetComponent <JointChainController>();
JointNum = Joints.Length;
EndJoint = Joints[Joints.Length - 1];
doSearch = false;
doMove = false;
}
