// Update is called once per frame
void Update()
{
if (Destination == null || HandObject == null)
{
return;
}
if (!Destination.IsConnected(this))
{
Destination.Connect(this);
}
if (ArmTree.Count == 0)
{
var startSoln = new List <float[]>
{
//GetComponent<IKSolver>().GetStartingAngles()
new float[armSolver.GetHomeAngles().Length]
};
var startNode = new Node(new Configuration(HandObject), null);
startNode.AddSolutionSteps(startSoln);
startNode.Point.AddJointAngles(armSolver.GetJointPose(startSoln));
ArmTree.Add(startNode);
Debug.Log("Got first point!");
}
if (GoalTree.Count == 0)
{
return;
}
if (doSearch)
{
RRTSearch();
}
}
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);
}
}