３次元のプランニングについて

前提・実現したいこと

3次元空間の物理的な挙動を考慮したブロックワールドにおけるプランニングを作っています。どこから手を付けていいかわからないので教えていただきたいです。java以外のフレームワークを使えるのなら使ってみたいので教えていただきたいです。

該当のソースコード

java

import java.util.*;

public class Planner{
 Vector operators;
 Random rand;
 Vector plan;

 public static void main(String argv[]){
	  List<String> ngoal=new ArrayList<String>();
	  List<String> nstart=new ArrayList<String>();
	 Scan3 sc=new Scan3();
	 sc.Inputting(ngoal, nstart);

  (new Planner()).start(ngoal,nstart);
 }

 Planner(){
  rand = new Random();
 }

 public void start(List<String> s1,List<String> s2){
  initOperators();
  Vector goalList     = initGoalList(s1);
  Vector initialState = initInitialState(s2);

  Hashtable theBinding = new Hashtable();
  plan = new Vector();
  planning(goalList,initialState,theBinding);

  
   for(int i = 0 ; i < plan.size() ; i++){
    Operator op = (Operator)plan.elementAt(i);
    System.out.println((op.instantiate(theBinding)).name);
   }
  }

 private boolean planning(Vector theGoalList,
                          Vector theCurrentState,
                          Hashtable theBinding){
  System.out.println("*** GOALS ***" + theGoalList);
  if(theGoalList.size() == 1){
   String aGoal = (String)theGoalList.elementAt(0);
   if(planningAGoal(aGoal,theCurrentState,theBinding,0) != -1){
    return true;
   } else {
    return false;
   }
  } else {
   String aGoal = (String)theGoalList.elementAt(0);
   int cPoint = 0;
   while(cPoint < operators.size()){
   
    Hashtable orgBinding = new Hashtable();
    for(Enumeration e = theBinding.keys() ; e.hasMoreElements();){
     String key = (String)e.nextElement();
     String value = (String)theBinding.get(key);
     orgBinding.put(key,value);
    }
    Vector orgState = new Vector();
    for(int i = 0; i < theCurrentState.size() ; i++){
     orgState.addElement(theCurrentState.elementAt(i));
    }

    int tmpPoint = planningAGoal(aGoal,theCurrentState,theBinding,cPoint);
   
    if(tmpPoint != -1){
     theGoalList.removeElementAt(0);
     System.out.println(theCurrentState);
     if(planning(theGoalList,theCurrentState,theBinding)){
     
      return true;
     } else {
      cPoint = tmpPoint;
      
      theGoalList.insertElementAt(aGoal,0);

      theBinding.clear();
      for(Enumeration e=orgBinding.keys();e.hasMoreElements();){
       String key = (String)e.nextElement();
       String value = (String)orgBinding.get(key);
       theBinding.put(key,value);
      }
      theCurrentState.removeAllElements();
      for(int i = 0 ; i < orgState.size() ; i++){
       theCurrentState.addElement(orgState.elementAt(i));
      }
     }
    } else {
     theBinding.clear();
     for(Enumeration e=orgBinding.keys();e.hasMoreElements();){
      String key = (String)e.nextElement();
      String value = (String)orgBinding.get(key);
      theBinding.put(key,value);
     }
     theCurrentState.removeAllElements();
     for(int i = 0 ; i < orgState.size() ; i++){
      theCurrentState.addElement(orgState.elementAt(i));
     }
     return false;
    }
   }
   return false;
  }
 }

 private int planningAGoal(String theGoal,Vector theCurrentState,
                           Hashtable theBinding,int cPoint){
  System.out.println("**"+theGoal);
  int size = theCurrentState.size();
  for(int i =  0; i < size ; i++){
   String aState = (String)theCurrentState.elementAt(i);
   if((new Unifier3()).unify(theGoal,aState,theBinding)){
    return 0;
   }
  }

  int randInt = Math.abs(rand.nextInt()) % operators.size();
  Operator op = (Operator)operators.elementAt(randInt);
  operators.removeElementAt(randInt);
  operators.addElement(op);

  for(int i = cPoint ; i < operators.size() ; i++){
   Operator anOperator = rename((Operator)operators.elementAt(i));
   
   Hashtable orgBinding = new Hashtable();
   for(Enumeration e = theBinding.keys() ; e.hasMoreElements();){
    String key = (String)e.nextElement();
    String value = (String)theBinding.get(key);
    orgBinding.put(key,value);
   }
   Vector orgState = new Vector();
   for(int j = 0; j < theCurrentState.size() ; j++){
    orgState.addElement(theCurrentState.elementAt(j));
   }
   Vector orgPlan = new Vector();
   for(int j = 0; j < plan.size() ; j++){
    orgPlan.addElement(plan.elementAt(j));
   }

   Vector addList = (Vector)anOperator.getAddList();
   for(int j = 0 ; j < addList.size() ; j++){
    if((new Unifier3()).unify(theGoal,
                             (String)addList.elementAt(j),
                             theBinding)){
     Operator newOperator = anOperator.instantiate(theBinding);
     Vector newGoals = (Vector)newOperator.getIfList();
     System.out.println(newOperator.name);
     if(planning(newGoals,theCurrentState,theBinding)){
      System.out.println(newOperator.name);
      plan.addElement(newOperator);
      theCurrentState =
       newOperator.applyState(theCurrentState);
      return i+1;
     } else {
      
      theBinding.clear();
      for(Enumeration e=orgBinding.keys();e.hasMoreElements();){
       String key = (String)e.nextElement();
       String value = (String)orgBinding.get(key);
       theBinding.put(key,value);
      }
      theCurrentState.removeAllElements();
      for(int k = 0 ; k < orgState.size() ; k++){
       theCurrentState.addElement(orgState.elementAt(k));
      }
      plan.removeAllElements();
      for(int k = 0 ; k < orgPlan.size() ; k++){
       plan.addElement(orgPlan.elementAt(k));
      }
     }
    }
   }
  }
  return -1;
 }

 int uniqueNum = 0;
 private Operator rename(Operator theOperator){
  Operator newOperator = theOperator.getRenamedOperator(uniqueNum);
  uniqueNum = uniqueNum + 1;
  return newOperator;
 }

 private Vector initGoalList(List<String>s1){
  Vector goalList = new Vector();
  for(int i=0;i<s1.size();i++) {
	  goalList.addElement(s1.get(i));
  }
  return goalList;
 }

 private Vector initInitialState(List<String>s1){
  Vector initialState = new Vector();
  for(int i=0;i<s1.size();i++) {
	  initialState.addElement(s1.get(i));
  }
  return initialState;
 }

 private void initOperators(){
  operators = new Vector();

  
}

Operatorクラスを省略

Uniferクラスを省略