def __init__(self, loadfile):
with open(loadfile, 'r') as stream:
params = yaml.load(stream)
# ----- General Setup ----- #
self.prefix = params["setup"]["prefix"]
self.feat_list = params["setup"]["feat_list"]
self.demo_spec = params["setup"]["demo_spec"]
# Openrave parameters for the environment.
model_filename = params["setup"]["model_filename"]
object_centers = params["setup"]["object_centers"]
self.environment = Environment(model_filename, object_centers)
# Learner setup.
constants = {}
constants["trajs_path"] = params["learner"]["trajs_path"]
constants["betas_list"] = params["learner"]["betas_list"]
constants["weight_vals"] = params["learner"]["weight_vals"]
constants["FEAT_RANGE"] = params["learner"]["FEAT_RANGE"]
self.learner = DemoLearner(self.feat_list, self.environment, constants)
if self.demo_spec == "simulate":
# Task setup.
pick = params["sim"]["task"]["start"]
place = params["sim"]["task"]["goal"]
self.start = np.array(pick)*(math.pi/180.0)
self.goal = np.array(place)*(math.pi/180.0)
self.goal_pose = None if params["sim"]["task"]["goal_pose"] == "None" else params["sim"]["task"]["goal_pose"]
self.T = params["sim"]["task"]["T"]
self.timestep = params["sim"]["task"]["timestep"]
self.weights = params["sim"]["task"]["feat_weights"]
# Planner Setup.
planner_type = params["sim"]["planner"]["type"]
if planner_type == "trajopt":
max_iter = params["sim"]["planner"]["max_iter"]
num_waypts = params["sim"]["planner"]["num_waypts"]
# Initialize planner and compute trajectory simulation.
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)]
plotTraj(self.environment.env, self.environment.robot, self.environment.bodies, self.traj[0].waypts, size=0.015,color=[0, 0, 1])
plotCupTraj(self.environment.env, self.environment.robot, self.environment.bodies, [self.traj[0].waypts[-1]],color=[0,1,0])
else:
data_path = params["setup"]["demo_dir"]
data_str = self.demo_spec.split("_")
data_dir = os.path.abspath(os.path.join(os.path.dirname( __file__ ), '../')) + data_path
if data_str[0] == "all":
file_str = data_dir + '/*task{}*.p'.format(data_str[1])
else:
file_str = data_dir + "/demo_ID{}_task{}*.p".format(data_str[0], data_str[1])
self.traj = [pickle.load(open(demo_file, "rb")) for demo_file in glob.glob(file_str)]
self.learner.learn_weights(self.traj)
def generate_traj_set(feat_list):
# Before calling this function, you need to decide what features you care
# about, from a choice of table, coffee, human, origin, and laptop.
pick = [104.2, 151.6, 183.8, 101.8, 224.2, 216.9, 310.8]
place = [210.8, 101.6, 192.0, 114.7, 222.2, 246.1, 322.0]
start = np.array(pick) * (math.pi / 180.0)
goal = np.array(place) * (math.pi / 180.0)
goal_pose = None
T = 20.0
timestep = 0.5
WEIGHTS_DICT = {
"table": [0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 7.0, 8.0],
"coffee": [0.0, 0.2, 0.4, 0.6, 0.8, 1.0],
"laptop": [0.0, 20.0, 21.0, 22.0, 24.0, 26.0, 30.0, 40.0],
"human": [0.0, 20.0, 21.0, 22.0, 24.0, 26.0, 30.0, 40.0],
"efficiency": [1.0]
}
# Openrave parameters for the environment.
model_filename = "jaco_dynamics"
object_centers = {
'HUMAN_CENTER': [-0.6, -0.55, 0.0],
'LAPTOP_CENTER': [-0.7929, -0.1, 0.0]
}
environment = Environment(model_filename, object_centers)
# Planner Setup
max_iter = 50
num_waypts = 5
feat_list = [x.strip() for x in feat_list.split(',')]
num_features = len(feat_list)
planner = TrajoptPlanner(feat_list, max_iter, num_waypts, environment)
# Construct set of weights of interest.
weights_span = [None] * num_features
for feat in range(0, num_features):
weights_span[feat] = WEIGHTS_DICT[feat_list[feat]]
weight_pairs = list(itertools.product(*weights_span))
weight_pairs = [list(i) for i in weight_pairs]
traj_rand = {}
for (w_i, weights) in enumerate(weight_pairs):
traj = planner.replan(start, goal, goal_pose, weights, T, timestep)
Phi = environment.featurize(traj.waypts, feat_list)
# Getting rid of bad, out-of-bounds trajectories
if any(phi < 0.0 for phi in Phi):
continue
traj = traj.waypts.tolist()
if repr(traj) not in traj_rand:
traj_rand[repr(traj)] = weights
here = os.path.abspath(os.path.join(os.path.dirname(__file__), '../../'))
savefile = "/data/traj_sets/traj_set_table_laptop.p"
pickle.dump(traj_rand, open(here + savefile, "wb"))
print "Saved in: ", savefile
print "Used the following weight-combos: ", weight_pairs
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 PathFollower(object):
"""
This class represents a node that computes an optimal path and moves the Jaco along.
Subscribes to:
/$prefix$/out/joint_angles - Jaco sensed joint angles
Publishes to:
/$prefix$/in/joint_velocity - Jaco commanded joint velocities
"""
def __init__(self):
# Create ROS node.
rospy.init_node("path_follower")
# Load parameters and set up subscribers/publishers.
self.load_parameters()
self.register_callbacks()
# Publish to ROS at 100hz.
r = rospy.Rate(100)
print "----------------------------------"
print "Moving robot, press ENTER to quit:"
while not rospy.is_shutdown():
if sys.stdin in select.select([sys.stdin], [], [], 0)[0]:
line = raw_input()
break
self.vel_pub.publish(ros_utils.cmd_to_JointVelocityMsg(self.cmd))
r.sleep()
print "----------------------------------"
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)
def register_callbacks(self):
"""
Sets up all the publishers/subscribers needed.
"""
# Create joint-velocity publisher.
self.vel_pub = rospy.Publisher(self.prefix + '/in/joint_velocity',
kinova_msgs.msg.JointVelocity,
queue_size=1)
# Create subscriber to joint_angles.
rospy.Subscriber(self.prefix + '/out/joint_angles',
kinova_msgs.msg.JointAngles,
self.joint_angles_callback,
queue_size=1)
def joint_angles_callback(self, msg):
"""
Reads the latest position of the robot and publishes an
appropriate torque command to move the robot to the target.
"""
# Read the current joint angles from the robot.
self.curr_pos = np.array([
msg.joint1, msg.joint2, msg.joint3, msg.joint4, msg.joint5,
msg.joint6, msg.joint7
]).reshape((7, 1))
# Convert to radians.
self.curr_pos = self.curr_pos * (math.pi / 180.0)
# Update cmd from PID based on current position.
self.cmd = self.controller.get_command(self.curr_pos)
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)
class PHRIInference():
"""
This class represents a node that moves the Jaco with PID control AND supports receiving human corrections online.
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("phri_inference")
# 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, press ENTER to quit:"
while not rospy.is_shutdown():
if sys.stdin in select.select([sys.stdin], [], [], 0)[0]:
line = raw_input()
break
self.vel_pub.publish(ros_utils.cmd_to_JointVelocityMsg(self.cmd))
r.sleep()
print "----------------------------------"
# Ask whether to save experimental data for pHRI corrections.
print "Type [yes/y/Y] if you'd like to save experimental data."
line = raw_input()
if (line is not "yes") and (line is not "Y") and (line is not "y"):
print "Not happy with recording. Terminating experiment."
else:
print "Please type in the ID number (e.g. [0/1/2/...])."
ID = raw_input()
print "Please type in the task number."
task = raw_input()
print "Saving experimental data to file..."
settings_string = "ID" + ID + "_" + self.feat_method + "_" + "_".join(
self.feat_list) + "_task" + task
weights_filename = "weights_" + settings_string
betas_filename = "betas_" + settings_string
betas_u_filename = "betas_u_" + settings_string
force_filename = "force_" + settings_string
interaction_pts_filename = "interaction_pts_" + settings_string
tracked_filename = "tracked_" + settings_string
deformed_filename = "deformed_" + settings_string
deformed_waypts_filename = "deformed_waypts_" + settings_string
replanned_filename = "replanned_" + settings_string
replanned_waypts_filename = "replanned_waypts_" + settings_string
updates_filename = "updates_" + settings_string
self.expUtil.pickle_weights(weights_filename)
self.expUtil.pickle_betas(betas_filename)
self.expUtil.pickle_betas_u(betas_u_filename)
self.expUtil.pickle_force(force_filename)
self.expUtil.pickle_interaction_pts(interaction_pts_filename)
self.expUtil.pickle_tracked_traj(tracked_filename)
self.expUtil.pickle_deformed_trajList(deformed_filename)
self.expUtil.pickle_deformed_wayptsList(deformed_waypts_filename)
self.expUtil.pickle_replanned_trajList(replanned_filename)
self.expUtil.pickle_replanned_wayptsList(replanned_waypts_filename)
self.expUtil.pickle_updates(updates_filename)
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.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)
def register_callbacks(self):
"""
Sets up all the publishers/subscribers needed.
"""
# Create joint-velocity publisher.
self.vel_pub = rospy.Publisher(self.prefix + '/in/joint_velocity',
kinova_msgs.msg.JointVelocity,
queue_size=1)
# Create subscriber to joint_angles.
rospy.Subscriber(self.prefix + '/out/joint_angles',
kinova_msgs.msg.JointAngles,
self.joint_angles_callback,
queue_size=1)
# Create subscriber to joint torques
rospy.Subscriber(self.prefix + '/out/joint_torques',
kinova_msgs.msg.JointTorque,
self.joint_torques_callback,
queue_size=1)
def joint_angles_callback(self, msg):
"""
Reads the latest position of the robot and publishes an
appropriate torque command to move the robot to the target.
"""
# Read the current joint angles from the robot.
self.curr_pos = np.array([
msg.joint1, msg.joint2, msg.joint3, msg.joint4, msg.joint5,
msg.joint6, msg.joint7
]).reshape((7, 1))
# Convert to radians.
self.curr_pos = self.curr_pos * (math.pi / 180.0)
# Update cmd from PID based on current position.
self.cmd = self.controller.get_command(self.curr_pos)
# Check is start/goal has been reached.
if self.controller.path_start_T is not None:
self.reached_start = True
self.expUtil.set_startT(self.controller.path_start_T)
if self.controller.path_end_T is not None:
self.reached_goal = True
self.expUtil.set_endT(self.controller.path_end_T)
# Update the experiment utils executed trajectory tracker.
if self.reached_start and not self.reached_goal:
timestamp = time.time() - self.controller.path_start_T
self.expUtil.update_tracked_traj(timestamp, self.curr_pos)
def joint_torques_callback(self, msg):
"""
Reads the latest torque sensed by the robot and records it for
plotting & analysis
"""
# Read the current joint torques from the robot.
torque_curr = np.array([
msg.joint1, msg.joint2, msg.joint3, msg.joint4, msg.joint5,
msg.joint6, msg.joint7
]).reshape((7, 1))
interaction = False
for i in range(7):
# Center torques around zero.
torque_curr[i][0] -= self.INTERACTION_TORQUE_THRESHOLD[i]
# Check if interaction was not noise.
if np.fabs(
torque_curr[i][0]
) > self.INTERACTION_TORQUE_EPSILON[i] and self.reached_start:
interaction = True
# If we experienced large enough interaction force, then learn.
if interaction:
if self.reached_start and not self.reached_goal:
timestamp = time.time() - self.controller.path_start_T
self.expUtil.update_tauH(timestamp, torque_curr)
self.expUtil.update_interaction_point(timestamp, self.curr_pos)
self.weights = self.learner.learn_weights(
self.traj, torque_curr, timestamp)
betas = self.learner.betas
betas_u = self.learner.betas_u
updates = self.learner.updates
self.traj = self.planner.replan(self.start,
self.goal,
self.goal_pose,
self.weights,
self.T,
self.timestep,
seed=self.traj_plan.waypts)
self.traj_plan = self.traj.downsample(self.planner.num_waypts)
self.controller.set_trajectory(self.traj)
# Update the experimental data with new weights and new betas.
timestamp = time.time() - self.controller.path_start_T
self.expUtil.update_weights(timestamp, self.weights)
self.expUtil.update_betas(timestamp, betas)
self.expUtil.update_betas_u(timestamp, betas_u)
self.expUtil.update_updates(timestamp, updates)
# Update the list of replanned plans with new trajectory plan.
self.expUtil.update_replanned_trajList(timestamp,
self.traj_plan.waypts)
# Update the list of replanned trajectory waypts with new trajectory.
self.expUtil.update_replanned_wayptsList(
timestamp, self.traj.waypts)
# Store deformed trajectory plan.
deformed_traj = self.learner.traj_deform.downsample(
self.planner.num_waypts)
self.expUtil.update_deformed_trajList(timestamp,
deformed_traj.waypts)
# Store deformed trajectory waypoints.
self.expUtil.update_deformed_wayptsList(
timestamp, self.learner.traj_deform.waypts)
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)