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)
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)
