after ewfvwefv
changed dunno what
Merge branch 'master' of https://github.com/mefox/robotautonomy_hw2

Conflicts:
	database/kinematics.c9e5215735aecb79c5312f66d75d8bff/ikfast61.Transform6D.0_1_3_4_5_6_f2.cpp
	database/kinematics.c9e5215735aecb79c5312f66d75d8bff/ikfast61.Transform6D.0_1_3_4_5_6_f2.pp
	hw2_search.py

PACKAGE_NAME = 'hw2'

import os
import copy
import time
import math
import numpy as np
np.random.seed(0)
import scipy

import collections
import Queue

import openravepy

curr_path = os.getcwd()
relative_ordata = '/models'
ordata_path_thispack = curr_path + relative_ordata

openrave_data_path = os.getenv('OPENRAVE_DATA', '')
openrave_data_paths = openrave_data_path.split(':')
if ordata_path_thispack not in openrave_data_paths:
  if openrave_data_path == '':
      os.environ['OPENRAVE_DATA'] = ordata_path_thispack
  else:
      datastr = str('%s:%s'%(ordata_path_thispack, openrave_data_path))
      os.environ['OPENRAVE_DATA'] = datastr

relative_ordatabase = '/database'
ordatabase_path_thispack = curr_path + relative_ordatabase
os.environ['OPENRAVE_DATABASE'] = ordatabase_path_thispack

openravepy.RaveInitialize(True, openravepy.DebugLevel.Fatal)
openravepy.misc.InitOpenRAVELogging()

TRANS_PER_DIR = 0.1

class RoboHandler:
  def __init__(self):
    self.openrave_init()
    self.problem_init()

  #######################################################
  # the usual initialization for openrave
  #######################################################
  def openrave_init(self):
    self.env = openravepy.Environment()
    self.env.SetViewer('qtcoin')
    self.env.GetViewer().SetName('HW2 Viewer')
    self.env.Load('models/%s.env.xml' %PACKAGE_NAME)
    # time.sleep(3) # wait for viewer to initialize. May be helpful to uncomment
    self.robot = self.env.GetRobots()[0]

    #set right wam as active manipulator
    with self.env:
      self.robot.SetActiveManipulator('right_wam');
      self.manip = self.robot.GetActiveManipulator()

      #set active indices to be right arm only
      self.robot.SetActiveDOFs(self.manip.GetArmIndices() )
      self.end_effector = self.manip.GetEndEffector()

  #######################################################
  # problem specific initialization
  #######################################################
  def problem_init(self):
    self.target_kinbody = self.env.GetKinBody("target")

    # create a grasping module
    self.gmodel = openravepy.databases.grasping.GraspingModel(self.robot, self.target_kinbody)

    # load grasps
    if not self.gmodel.load():
      self.gmodel.autogenerate()

    self.grasps = self.gmodel.grasps
    self.graspindices = self.gmodel.graspindices

    # load ikmodel
    self.ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(self.robot,iktype=openravepy.IkParameterization.Type.Transform6D)
    if not self.ikmodel.load():
      self.ikmodel.autogenerate()

    # create taskmanip
    self.taskmanip = openravepy.interfaces.TaskManipulation(self.robot)

    # move left arm out of way
    self.robot.SetDOFValues(np.array([4,2,0,-1,0,0,0]),self.robot.GetManipulator('left_wam').GetArmIndices() )

  #######################################################
  # Simpler search problem - uses breadth first search algorithm
  #######################################################
  def run_simple_problem(self):
    self.robot.GetController().Reset()

    # move hand to preshape of grasp
    # --- important --
    # I noted they were all the same, otherwise you would need to do this separately for each grasp!
    with self.env:
      self.robot.SetDOFValues(self.grasps[0][self.graspindices['igrasppreshape']], self.manip.GetGripperIndices()) # move to preshape

    self.init_transition_arrays()
    goal = [ 0.93422058, -1.10221021, -0.2       ,  2.27275587, -0.22977831, -1.09393251, -2.23921746]
    with self.env:
      self.robot.SetActiveDOFValues([ 1.23, -1.10, -0.3,  2.37, -0.23, -1.29, -2.23])

    # get the trajectory!
    traj = self.search_to_goal_breadthfirst([goal])

    with self.env:
      self.robot.SetActiveDOFValues([ 1.23, -1.10, -0.3,  2.37, -0.23, -1.29, -2.23])

    self.robot.GetController().SetPath(traj)
    self.robot.WaitForController(0)
    self.taskmanip.CloseFingers()

  #######################################################
  # Harder search problem - uses A* algorithm
  #######################################################
  def run_difficult_problem(self):
    self.robot.GetController().Reset()

    # move hand to preshape of grasp
    # --- important --
    # I noted they were all the same, otherwise you would need to do this separately for each grasp!
    with self.env:
      self.robot.SetDOFValues(self.grasps[0][self.graspindices['igrasppreshape']], self.manip.GetGripperIndices()) # move to preshape

    self.init_transition_arrays()
    #goals = self.get_goal_dofs(7,1)
    goals = np.array([[ 0.93422058, -1.10221021, -0.2       ,  2.27275587, -0.22977831, -1.09393251, -2.23921746],
       [ 1.38238176, -1.05017481,  0.        ,  1.26568204,  0.15001448,  1.32813949, -0.06022621],
       [ 1.16466262, -1.02175153, -0.3       ,  1.26568204, -2.62343746, -1.43813577, -0.37988181],
       [ 3.45957137, -0.48619817,  0.        ,  2.0702298 , -1.12033301, -1.33241556,  1.85646563],
       [ 1.65311863, -1.17157253,  0.4       ,  2.18692683, -2.38248898,  0.73272595, -0.23680544],
       [ 1.59512823, -1.07309638,  0.5       ,  2.26315055,  0.57257592, -1.15576369, -0.30723627],
       [ 1.67038884, -1.16082512,  0.4       ,  2.05339849, -2.0205527 ,  0.54970211, -0.4386743 ]])

    with self.env:
      self.robot.SetActiveDOFValues([5.459, -0.981,  -1.113,  1.473 , -1.124, -1.332,  1.856])

    # get the trajectory!
    traj = self.search_to_goal_astar(goals)

    with self.env:
      self.robot.SetActiveDOFValues([5.459, -0.981,  -1.113,  1.473 , -1.124, -1.332,  1.856])

    self.robot.GetController().SetPath(traj)
    self.robot.WaitForController(0)
    self.taskmanip.CloseFingers()

  #######################################################
  # finds the arm configurations (in cspace) that correspond
  # to valid grasps
  # num_goal: number of grasps to consider
  # num_dofs_per_goal: number of IK solutions per grasp
  #######################################################
  def get_goal_dofs(self, num_goals=1, num_dofs_per_goal=1):
    validgrasps,validindices = self.gmodel.computeValidGrasps(returnnum=num_goals)

    curr_IK = self.robot.GetActiveDOFValues()

    goal_dofs = np.array([])
    for grasp, graspindices in zip(validgrasps, validindices):
      Tgoal = self.gmodel.getGlobalGraspTransform(grasp, collisionfree=True)
      sols = self.manip.FindIKSolutions(Tgoal, openravepy.IkFilterOptions.CheckEnvCollisions)

      # magic that makes sols only the unique elements - sometimes there are multiple IKs
      sols = np.unique(sols.view([('',sols.dtype)]*sols.shape[1])).view(sols.dtype).reshape(-1,sols.shape[1])
      sols_scores = []
      for sol in sols:
        sols_scores.append( (sol, np.linalg.norm(sol-curr_IK)) )

      # sort by closest to current IK
      sols_scores.sort(key=lambda tup:tup[1])
      sols = np.array([x[0] for x in sols_scores])

      # sort randomly
      #sols = np.random.permutation(sols)

      #take up to num_dofs_per_goal
      last_ind = min(num_dofs_per_goal, sols.shape[0])
      goal_dofs = np.append(goal_dofs,sols[0:last_ind])

    goal_dofs = goal_dofs.reshape(goal_dofs.size/7, 7)

    return goal_dofs

  ### TODO ###
  #######################################################
  # DEPTH FIRST SEARCH
  # find a path from the current configuration to ANY goal in goals
  # goals: list of possible goal configurations
  # RETURN: a trajectory to the goal
  #######################################################
  ##########MARSH###############
  def search_to_goal_depthfirst(self, goals):
    visited_nodes = {}
    nodes = Queue.LifoQueue()
	nodes.put(self.robot.GetActiveDOFValues())
	print 'test'

    for g in goals: #for each of the goal states
        goal_reached = False
	    while not nodes.empty() and not goal_reached: #while the queue has nodes
		    currentNode = nodes.get() #pop the next node
            if currentNode == g: #check if we reached the current goal
                goal_reached = True
                continue #jump out of the loop

		    neighbors = transition_config(currentNode)

            for c in neighbors:
                if not c in visited_nodes:
                    visited_nodes[c] = currentNode
                    nodes.put(c)

		    print currentNode
        print 'Found goal' + g
	return 0

  ### TODO ###
  #######################################################
  # BREADTH FIRST SEARCH
  # find a path from the current configuration to ANY goal in goals
  # goals: list of possible goal configurations
  # RETURN: a trajectory to the goal
  #######################################################
  def search_to_goal_breadthfirst(self, goals):
    return

  ### TODO ###
  #######################################################
  # A* SEARCH
  # find a path from the current configuration to ANY goal in goals
  # goals: list of possible goal configurations
  # RETURN: a trajectory to the goal
  #######################################################
  def search_to_goal_astar(self, goals):
    return

  ### TODO ###  (not required but I found it useful)
  #######################################################
  # Pick a heuristic for
  #######################################################
  def config_to_priorityqueue_tuple(self, dist, config, goals):
    # you can use either of these - make sure to replace the 0 with your
    # priority queue value!
    return (0.0, config.tolist())
    return (0.0, self.convert_for_dict(config))

  #######################################################
  # Convert to and from numpy array to a hashable function
  #######################################################
  def convert_for_dict(self, item):
    return tuple(np.int_(item*100))
    #return tuple(item)

  def convert_from_dictkey(self, item):
    return np.array(item)/100.
    #return np.array(item)

  ### TODO ###  (not required but I found it useful)
  #######################################################
  # Initialize the movements you can apply in any direction
  # Don't forget to use TRANS_PER_DIR - the max distance you
  # can move any joint in a step (defined above)
  #######################################################
  def init_transition_arrays(self):
	positive_transition = np.identity(7)*TRANS_PER_DIR;
	negative_transition= positive_transition*-1;
	self.transition_arrays = np.concatenate((positive_transition, negative_transition), axis = 0)

    #self.transition_arrays = [[TRANS_PER_DIR,0,0,0,0,0,0],[0,TRANS_PER_DIR,0,0,0,0,0],[0,0,TRANS_PER_DIR,0,0,0,0],[0,0,0,TRANS_PER_DIR,0,0,0],[0,0,0,0,TRANS_PER_DIR,0,0],[0,0,0,0,0,TRANS_PER_DIR,0],[0,0,0,0,0,0,TRANS_PER_DIR]]
    return

  ### TODO ###  (not required but I found it useful)
  #######################################################
  # Take the current configuration and apply each of your
  # transition arrays to it
  #######################################################
  def transition_config(self, config):
<<<<<<< HEAD
    new_configs = [config+self.transition_arrays(1),]
=======
    ######## SSR  ######
    new_configs = np.array([]) #define blank array
    for c in self.transition_arrays: #loop through columns
        new_configs = np.concatenate((new_configs, c + config), axis = 0) #assemble new configuration
    new_configs = np.reshape(new_configs, (14, 7)) #reshape

>>>>>>> 1dbe310e092b3a8c42ed6d09ab1796cbba813aa0
    return new_configs

  #######################################################
  # Takes in a list of points, and creates a trajectory
  # that goes between them
  #######################################################
  def points_to_traj(self, points):
    traj = openravepy.RaveCreateTrajectory(self.env,'')
    traj.Init(self.robot.GetActiveConfigurationSpecification())
    for idx,point in enumerate(points):
      traj.Insert(idx,point)
    openravepy.planningutils.RetimeActiveDOFTrajectory(traj,self.robot,hastimestamps=False,maxvelmult=1,maxaccelmult=1,plannername='ParabolicTrajectoryRetimer')
    return traj

  ### TODO ###  (not required but I found it useful)
  #######################################################
  # minimum distance from config to any goal in goals
  # distance metric: euclidean
  # returns the distance AND closest goal
  #######################################################
  def min_euclid_dist_to_goals(self, config, goals):
    # replace the 0 and goal with the distance and closest goal
    return 0, goals[0]

  ### TODO ###  (not required but I found it useful)
  #######################################################
  # minimum distance from config to any goal in goals
  # distance metric: manhattan
  # returns the distance AND closest goal
  #######################################################
  def min_manhattan_dist_to_goals(self, config, goals):
    # replace the 0 and goal with the distance and closest goal
    return 0, goals[0]

  #######################################################
  # close the fingers when you get to the grasp position
  #######################################################
  def close_fingers(self):
    self.taskmanip.CloseFingers()
    self.robot.WaitForController(0) #ensures the robot isn't moving anymore
    #self.robot.Grab(target) #attaches object to robot, so moving the robot will move the object now

if __name__ == '__main__':
<<<<<<< HEAD
	robo = RoboHandler()
	temp_goal = [ 0.93422058, -1.10221021, -0.2       ,  2.27275587, -0.22977831, -1.09393251, -2.23921746]
	robo.search_to_goal_depthfirst(temp_goal)
  #run_simple_problem() #runs the simple problem
  #time.sleep(10000) #to keep the openrave window open

./database
Updated trajectory function