// 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); } }