def load_parameters(self): """ Loading parameters and setting up variables from the ROS environment. """ # ----- General Setup ----- # self.prefix = rospy.get_param("setup/prefix") self.start = np.array(rospy.get_param("setup/start"))*(math.pi/180.0) self.T = rospy.get_param("setup/T") self.timestep = rospy.get_param("setup/timestep") self.save_dir = rospy.get_param("setup/save_dir") # Openrave parameters for the environment. model_filename = rospy.get_param("setup/model_filename") object_centers = rospy.get_param("setup/object_centers") self.environment = Environment(model_filename, object_centers) # ----- Controller Setup ----- # # Retrieve controller specific parameters. controller_type = rospy.get_param("controller/type") if controller_type == "pid": # P, I, D gains. P = rospy.get_param("controller/p_gain") * np.eye(7) I = rospy.get_param("controller/i_gain") * np.eye(7) D = rospy.get_param("controller/d_gain") * np.eye(7) # Stores proximity threshold. epsilon = rospy.get_param("controller/epsilon") # Stores maximum COMMANDED joint torques. MAX_CMD = rospy.get_param("controller/max_cmd") * np.eye(7) self.controller = PIDController(P, I, D, epsilon, MAX_CMD) else: raise Exception('Controller {} not implemented.'.format(controller_type)) # Tell controller to move to start. self.controller.set_trajectory(Trajectory([self.start], [0.0])) # Stores current COMMANDED joint torques. self.cmd = np.eye(7) # Utilities for recording data. self.expUtil = experiment_utils.ExperimentUtils(self.save_dir)
def load_parameters(self): """ Loading parameters and setting up variables from the ROS environment. """ # ----- General Setup ----- # self.prefix = rospy.get_param("setup/prefix") pick = rospy.get_param("setup/start") place = rospy.get_param("setup/goal") self.start = np.array(pick) * (math.pi / 180.0) self.goal = np.array(place) * (math.pi / 180.0) self.goal_pose = None if rospy.get_param( "setup/goal_pose") == "None" else rospy.get_param( "setup/goal_pose") self.T = rospy.get_param("setup/T") self.timestep = rospy.get_param("setup/timestep") self.feat_list = rospy.get_param("setup/feat_list") self.weights = rospy.get_param("setup/feat_weights") # Openrave parameters for the environment. model_filename = rospy.get_param("setup/model_filename") object_centers = rospy.get_param("setup/object_centers") self.environment = Environment(model_filename, object_centers) # ----- Planner Setup ----- # # Retrieve the planner specific parameters. planner_type = rospy.get_param("planner/type") if planner_type == "trajopt": max_iter = rospy.get_param("planner/max_iter") num_waypts = rospy.get_param("planner/num_waypts") # Initialize planner and compute trajectory to track. self.planner = TrajoptPlanner(self.feat_list, max_iter, num_waypts, self.environment) else: raise Exception('Planner {} not implemented.'.format(planner_type)) self.traj = self.planner.replan(self.start, self.goal, self.goal_pose, self.weights, self.T, self.timestep) # Save the intermediate target configuration. self.curr_pos = None # ----- Controller Setup ----- # # Retrieve controller specific parameters. controller_type = rospy.get_param("controller/type") if controller_type == "pid": # P, I, D gains. P = rospy.get_param("controller/p_gain") * np.eye(7) I = rospy.get_param("controller/i_gain") * np.eye(7) D = rospy.get_param("controller/d_gain") * np.eye(7) # Stores proximity threshold. epsilon = rospy.get_param("controller/epsilon") # Stores maximum COMMANDED joint torques. MAX_CMD = rospy.get_param("controller/max_cmd") * np.eye(7) self.controller = PIDController(P, I, D, epsilon, MAX_CMD) else: raise Exception( 'Controller {} not implemented.'.format(controller_type)) # Planner tells controller what plan to follow. self.controller.set_trajectory(self.traj) # Stores current COMMANDED joint torques. self.cmd = np.eye(7)
class FeatureElicitator(): """ This class represents a node that moves the Jaco with PID control AND supports receiving human corrections online. Additionally, it implements a protocol for elicitating novel features from human input. Subscribes to: /$prefix$/out/joint_angles - Jaco sensed joint angles /$prefix$/out/joint_torques - Jaco sensed joint torques Publishes to: /$prefix$/in/joint_velocity - Jaco commanded joint velocities """ def __init__(self): # Create ROS node. rospy.init_node("feature_elicitator") # Load parameters and set up subscribers/publishers. self.load_parameters() self.register_callbacks() # Start admittance control mode. ros_utils.start_admittance_mode(self.prefix) # Publish to ROS at 100hz. r = rospy.Rate(100) print "----------------------------------" print "Moving robot, type Q to quit:" while not rospy.is_shutdown(): if sys.stdin in select.select([sys.stdin], [], [], 0)[0]: line = raw_input() if line == "q" or line == "Q" or line == "quit": break self.vel_pub.publish(ros_utils.cmd_to_JointVelocityMsg(self.cmd)) r.sleep() print "----------------------------------" ros_utils.stop_admittance_mode(self.prefix) def load_parameters(self): """ Loading parameters and setting up variables from the ROS environment. """ # ----- General Setup ----- # self.prefix = rospy.get_param("setup/prefix") pick = rospy.get_param("setup/start") place = rospy.get_param("setup/goal") self.start = np.array(pick)*(math.pi/180.0) self.goal = np.array(place)*(math.pi/180.0) self.goal_pose = None if rospy.get_param("setup/goal_pose") == "None" else rospy.get_param("setup/goal_pose") self.T = rospy.get_param("setup/T") self.timestep = rospy.get_param("setup/timestep") self.save_dir = rospy.get_param("setup/save_dir") self.INTERACTION_TORQUE_THRESHOLD = rospy.get_param("setup/INTERACTION_TORQUE_THRESHOLD") self.INTERACTION_TORQUE_EPSILON = rospy.get_param("setup/INTERACTION_TORQUE_EPSILON") self.CONFIDENCE_THRESHOLD = rospy.get_param("setup/CONFIDENCE_THRESHOLD") self.N_QUERIES = rospy.get_param("setup/N_QUERIES") self.nb_layers = rospy.get_param("setup/nb_layers") self.nb_units = rospy.get_param("setup/nb_units") # Openrave parameters for the environment. model_filename = rospy.get_param("setup/model_filename") object_centers = rospy.get_param("setup/object_centers") feat_list = rospy.get_param("setup/feat_list") weights = rospy.get_param("setup/feat_weights") FEAT_RANGE = rospy.get_param("setup/FEAT_RANGE") feat_range = [FEAT_RANGE[feat_list[feat]] for feat in range(len(feat_list))] LF_dict = rospy.get_param("setup/LF_dict") self.environment = Environment(model_filename, object_centers, feat_list, feat_range, np.array(weights), LF_dict) # ----- Planner Setup ----- # # Retrieve the planner specific parameters. planner_type = rospy.get_param("planner/type") if planner_type == "trajopt": max_iter = rospy.get_param("planner/max_iter") num_waypts = rospy.get_param("planner/num_waypts") # Initialize planner and compute trajectory to track. self.planner = TrajoptPlanner(max_iter, num_waypts, self.environment) else: raise Exception('Planner {} not implemented.'.format(planner_type)) self.traj = self.planner.replan(self.start, self.goal, self.goal_pose, self.T, self.timestep) self.traj_plan = self.traj.downsample(self.planner.num_waypts) # Track if you have reached the start/goal of the path. self.reached_start = False self.reached_goal = False self.feature_learning_mode = False self.interaction_mode = False # Save the intermediate target configuration. self.curr_pos = None # Track data and keep stored. self.interaction_data = [] self.interaction_time = [] self.feature_data = [] self.track_data = False # ----- Controller Setup ----- # # Retrieve controller specific parameters. controller_type = rospy.get_param("controller/type") if controller_type == "pid": # P, I, D gains. P = rospy.get_param("controller/p_gain") * np.eye(7) I = rospy.get_param("controller/i_gain") * np.eye(7) D = rospy.get_param("controller/d_gain") * np.eye(7) # Stores proximity threshold. epsilon = rospy.get_param("controller/epsilon") # Stores maximum COMMANDED joint torques. MAX_CMD = rospy.get_param("controller/max_cmd") * np.eye(7) self.controller = PIDController(P, I, D, epsilon, MAX_CMD) else: raise Exception('Controller {} not implemented.'.format(controller_type)) # Planner tells controller what plan to follow. self.controller.set_trajectory(self.traj) # Stores current COMMANDED joint torques. self.cmd = np.eye(7) # ----- Learner Setup ----- # constants = {} constants["step_size"] = rospy.get_param("learner/step_size") constants["P_beta"] = rospy.get_param("learner/P_beta") constants["alpha"] = rospy.get_param("learner/alpha") constants["n"] = rospy.get_param("learner/n") self.feat_method = rospy.get_param("learner/type") self.learner = PHRILearner(self.feat_method, self.environment, constants)
def load_parameters(self): """ Loading parameters and setting up variables from the ROS environment. """ # ----- General Setup ----- # self.prefix = rospy.get_param("setup/prefix") pick = rospy.get_param("setup/start") place = rospy.get_param("setup/goal") self.start = np.array(pick) * (math.pi / 180.0) self.goal = np.array(place) * (math.pi / 180.0) self.goal_pose = None if rospy.get_param( "setup/goal_pose") == "None" else rospy.get_param( "setup/goal_pose") self.T = rospy.get_param("setup/T") self.timestep = rospy.get_param("setup/timestep") self.save_dir = rospy.get_param("setup/save_dir") self.feat_list = rospy.get_param("setup/feat_list") self.weights = rospy.get_param("setup/feat_weights") self.INTERACTION_TORQUE_THRESHOLD = rospy.get_param( "setup/INTERACTION_TORQUE_THRESHOLD") self.INTERACTION_TORQUE_EPSILON = rospy.get_param( "setup/INTERACTION_TORQUE_EPSILON") # Openrave parameters for the environment. model_filename = rospy.get_param("setup/model_filename") object_centers = rospy.get_param("setup/object_centers") self.environment = Environment(model_filename, object_centers) # ----- Planner Setup ----- # # Retrieve the planner specific parameters. planner_type = rospy.get_param("planner/type") if planner_type == "trajopt": max_iter = rospy.get_param("planner/max_iter") num_waypts = rospy.get_param("planner/num_waypts") # Initialize planner and compute trajectory to track. self.planner = TrajoptPlanner(self.feat_list, max_iter, num_waypts, self.environment) else: raise Exception('Planner {} not implemented.'.format(planner_type)) self.traj = self.planner.replan(self.start, self.goal, self.goal_pose, self.weights, self.T, self.timestep) self.traj_plan = self.traj.downsample(self.planner.num_waypts) # Track if you have reached the start/goal of the path. self.reached_start = False self.reached_goal = False # Save the intermediate target configuration. self.curr_pos = None # ----- Controller Setup ----- # # Retrieve controller specific parameters. controller_type = rospy.get_param("controller/type") if controller_type == "pid": # P, I, D gains. P = rospy.get_param("controller/p_gain") * np.eye(7) I = rospy.get_param("controller/i_gain") * np.eye(7) D = rospy.get_param("controller/d_gain") * np.eye(7) # Stores proximity threshold. epsilon = rospy.get_param("controller/epsilon") # Stores maximum COMMANDED joint torques. MAX_CMD = rospy.get_param("controller/max_cmd") * np.eye(7) self.controller = PIDController(P, I, D, epsilon, MAX_CMD) else: raise Exception( 'Controller {} not implemented.'.format(controller_type)) # Planner tells controller what plan to follow. self.controller.set_trajectory(self.traj) # Stores current COMMANDED joint torques. self.cmd = np.eye(7) # ----- Learner Setup ----- # constants = {} constants["UPDATE_GAINS"] = rospy.get_param("learner/UPDATE_GAINS") constants["MAX_WEIGHTS"] = rospy.get_param("learner/MAX_WEIGHTS") constants["FEAT_RANGE"] = rospy.get_param("learner/FEAT_RANGE") constants["P_beta"] = rospy.get_param("learner/P_beta") constants["alpha"] = rospy.get_param("learner/alpha") constants["n"] = rospy.get_param("learner/n") self.feat_method = rospy.get_param("learner/type") self.learner = PHRILearner(self.feat_method, self.feat_list, self.environment, constants) # ---- Experimental Utils ---- # self.expUtil = experiment_utils.ExperimentUtils(self.save_dir) # Update the list of replanned plans with new trajectory plan. self.expUtil.update_replanned_trajList(0.0, self.traj_plan.waypts) # Update the list of replanned waypoints with new waypoints. self.expUtil.update_replanned_wayptsList(0.0, self.traj.waypts)