def __init__(self, params):
'''Initialize internal variables'''
PIDController.__init__(self,params)
# This variable should contain a list of vectors
# calculated from sensor readings. It is used by
# the supervisor to draw & debug the controller's
# behaviour
self.vectors = []
def set_parameters(self, params):
"""Set PID values and sensor poses.
The params structure is expected to have sensor poses in the robot's
reference frame as ``params.sensor_poses``.
"""
PIDController.set_parameters(self, params)
self.sensor_poses = params.sensor_poses
def __init__(self, params):
"""Initialize internal variables"""
PIDController.__init__(self,params)
# These two angles are used by the supervisor
# to debug the controller's behaviour, and contain
# the headings as returned by the two subcontrollers.
self.goal_angle = 0
self.away_angle = 0
def set_parameters(self, params):
"""Set PID values and sensor poses.
The params structure is expected to have sensor poses in the robot's
reference frame as ``params.sensor_poses``.
"""
PIDController.set_parameters(self,params)
self.sensor_poses = params.sensor_poses
# Week 4 assigment
# Set the weigths here
self.weights = [1]*len(self.sensor_poses)
def set_parameters(self, params):
"""Set PID values, sensor poses, direction and distance.
The params structure is expected to have sensor poses in the robot's
reference frame as ``params.sensor_poses``, the direction of wall
following (either 'right' for clockwise or 'left' for anticlockwise)
as ``params.direction`` and the desired distance to the wall
to maintain as ``params.distance``.
"""
PIDController.set_parameters(self, params)
self.sensor_poses = params.sensor_poses
self.direction = params.direction
self.distance = params.distance
def set_parameters(self, params):
"""Set PID values, sensor poses, direction and distance.
The params structure is expected to have sensor poses in the robot's
reference frame as ``params.sensor_poses``, the direction of wall
following (either 'right' for clockwise or 'left' for anticlockwise)
as ``params.direction`` and the desired distance to the wall
to maintain as ``params.distance``.
"""
PIDController.set_parameters(self, params)
self.sensor_poses = params.sensor_poses
self.direction = params.direction
self.distance = params.distance
def execute(self, state, dt):
v, w = PIDController.execute(self, state, dt)
# #TODO check this
# velocity should depend on distance to the goal
# The goal:
# x_g, y_g = state.goal.x, state.goal.y
# # The robot:
# x_r, y_r, theta = state.pose
# print("current Robot pose: ({}, {}, {})".format(x_r, y_r, theta), file=sys.stderr)
# # distance between goal and robot in x - direction
# u_x = x_g - x_r
# # distance between goal and robot in y - direction
# u_y = y_g - y_r
# # distance between robot and goal
# # dist = numpy.linalg.norm(numpy.array([u_x,u_y]))
# # v = abs(w) + dist
# v = state.velocity
#
print("current v, w: ({}, {})".format(v, w), file=sys.stderr)
return v, w
def execute(self, state, dt):
v, w = PIDController.execute(self, state, dt)
# TODO check this regulation of velocity
# dmin = min(state.sensor_distances)
# v *= ((dmin - 0.04)/0.26)**2
return v, w
def execute(self, state, dt):
v, w = PIDController.execute(self, state, dt)
# Week 5 Assigment Code goes here:
# End Week 5 Assigment
return v, w
def execute(self, state, dt):
v, w = PIDController.execute(self, state, dt)
# Week 5 code
#
#
return v, w
def set_parameters(self, params):
"""Set PID values and sensor poses.
The params structure is expected to have sensor poses in the robot's
reference frame as ``params.sensor_poses``.
"""
PIDController.set_parameters(self,params)
self.sensor_poses = params.sensor_poses
# Now we know the poses, it makes sense to also
# calculate the weights
#self.weights = [(math.cos(p.theta)+1.5) for p in self.sensor_poses]
self.weights = [1.0, 1.0, 0.5, 1.0, 1.0]
# Normalizing weights
ws = sum(self.weights)
self.weights = [w/ws for w in self.weights]
def execute(self, state, dt):
v, w = PIDController.execute(self, state, dt)
# Week 5 code
#
#
return v, w
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 execute(self, state, dt):
v, w = PIDController.execute(self, state, dt)
dmin = min(state.sensor_distances)
v *= ((dmin - 0.04)/0.26)**2
#
return v, w
def execute(self, state, dt):
v, w = PIDController.execute(self, state, dt)
dmin = min(state.sensor_distances)
v *= ((dmin - 0.04) / 0.26)**2
#
return v, w
def execute(self, state, dt):
self._timer += 1
v, w = PIDController.execute(self, state, dt)
d, i = min(zip(state.sensor_distances,
range(len(state.sensor_distances))))
if i in (1, 2, 3):
ret = -v, w
else:
ret = v, w
print self._timer
if d > self.params.distance:
# if self._timer >= 23:
self._timer = 0
self.done = True
return ret
def set_parameters(self, params):
"""
Set parameters.
"""
PIDController.set_parameters(self, params)
def __init__(self, params):
"""Initialize internal variables"""
PIDController.__init__(self,params)
self.params = params
def restart(self):
# super(GoBack, self).restart()
PIDController.restart(self)
self.done = False
def restart(self):
"""Reset internal state"""
PIDController.restart(self)
def execute(self, state, dt):
v, w = PIDController.execute(self, state, dt)
#print 'Move to point ', (self.params.ga_path[point_cnt][0], self.params.ga_path[point_cnt][1])
return v, w
def get_heading_angle(self, state):
return PIDController.get_heading_angle(self,state)
def restart(self):
"""Reset internal state"""
PIDController.restart(self)
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 __init__(self, params):
"""Initialize internal variables"""
PIDController.__init__(self,params)
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 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.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 __init__(self, params):
'''Initialize internal variables'''
PIDController.__init__(self,params)
def __init__(self, params):
"""Initialize internal variables"""
PIDController.__init__(self,params)
self.params = params
print params.ga_path
def __init__(self, params):
"""Initialize internal variables"""
PIDController.__init__(self, params)
self._timer = 0
self.done = False
def execute(self, state, dt):
v, w = PIDController.execute(self, state, dt)
return v, w
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)
class DemoRecorder(object):
"""
This class represents a node that moves the Jaco to a start position and
allows the human to give a demonstration for recording.
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("demo_recorder")
# Load parameters and set up subscribers/publishers.
self.load_parameters()
self.register_callbacks()
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 "----------------------------------"
# Process and save the recording.
raw_demo = self.expUtil.tracked_traj[:,1:8]
# Trim ends of waypoints and create Trajectory.
lo = 0
hi = raw_demo.shape[0] - 1
while np.linalg.norm(raw_demo[lo] - raw_demo[lo + 1]) < 0.01 and lo < hi:
lo += 1
while np.linalg.norm(raw_demo[hi] - raw_demo[hi - 1]) < 0.01 and hi > 0:
hi -= 1
waypts = raw_demo[lo:hi+1, :]
waypts_time = np.linspace(0.0, self.T, waypts.shape[0])
traj = Trajectory(waypts, waypts_time)
# Downsample/Upsample trajectory to fit desired timestep and T.
num_waypts = int(self.T / self.timestep) + 1
if num_waypts < len(traj.waypts):
demo = traj.downsample(int(self.T / self.timestep) + 1)
else:
demo = traj.upsample(int(self.T / self.timestep) + 1)
# Decide whether to save trajectory
openrave_utils.plotTraj(self.environment.env, self.environment.robot,
self.environment.bodies, demo.waypts, size=0.015, color=[0, 0, 1])
print "Type [yes/y/Y] if you're happy with the demonstration."
line = raw_input()
if (line is not "yes") and (line is not "Y") and (line is not "y"):
print "Not happy with demonstration. 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 (e.g. [0/1/2/...])."
task = raw_input()
filename = "demo" + "_ID" + ID + "_task" + task
savefile = self.expUtil.get_unique_filepath("demos",filename)
pickle.dump(demo, open(savefile, "wb" ))
print "Saved demonstration in {}.".format(savefile)
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")
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 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.
curr_pos = np.array([msg.joint1,msg.joint2,msg.joint3,msg.joint4,msg.joint5,msg.joint6,msg.joint7]).reshape((7,1))
# Convert to radians.
curr_pos = curr_pos*(math.pi/180.0)
# Update cmd from PID based on current position.
if self.controller.path_start_T is not None:
# Allow the person to move the end effector with no control resistance.
self.cmd = np.zeros((7,7))
# Update the experiment utils executed trajectory tracker.
timestamp = time.time() - self.controller.path_start_T
self.expUtil.update_tracked_traj(timestamp, curr_pos)
else:
self.cmd = self.controller.get_command(curr_pos)
