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)
