def execute_loop(self):
r = rospy.Rate(100)
while not rospy.is_shutdown() and not (self.reached_goal
and self.reached_start):
self.vel_pub.publish(ros_utils.cmd_to_JointVelocityMsg(self.cmd))
r.sleep()
def start_ROS(self):
"""
Creates the ROS node, publishers, and subscribers for the real robot.
"""
if self.admittance_flag:
# start admittance control mode
self.start_admittance_mode()
# ---- ROS Setup ---- #
rospy.init_node("jaco_controller")
# create joint-velocity publisher
self.vel_pub = rospy.Publisher(prefix + '/in/joint_velocity',
kinova_msgs.msg.JointVelocity,
queue_size=1)
# create subscriber to joint_angles
rospy.Subscriber(prefix + '/out/joint_angles',
kinova_msgs.msg.JointAngles,
self.joint_angles_callback,
queue_size=1)
# create subscriber to joint_torques
rospy.Subscriber(prefix + '/out/joint_torques',
kinova_msgs.msg.JointTorque,
self.joint_torques_callback,
queue_size=1)
# 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 "----------------------------------"
if self.admittance_flag:
# end admittance control mode
self.stop_admittance_mode()
def execute_cb(self, goal):
""" Main callback for the trajectory action
Execute the trajectory, stop if premted, report success when done
goal: a :control_msgs.msg.FollowJointTrajectoryActionGoal: message containing the trajectory and tolerance info
"""
#TODO use the tolerance variables available:
# g = control_msgs.msg.FollowJointTrajectoryGoal()
# g.trajectory
# g.goal_time_tolerance
# g.goal_tolerance
# g.path_tolerance
# The trajectory execution is true unless set false
success = True
# publish info to the console for the user
rospy.loginfo('{}: Recieve joint trajectory request'.format(
self._action_name))
# start executing the action
self._controller.load_trajectory(goal.trajectory)
r = rospy.Rate(100)
while not rospy.is_shutdown() and not (self._controller.reached_goal
and
self._controller.reached_start):
rospy.loginfo_throttle(
5, "reached goal {}, reached_start {}".format(
self._controller.reached_goal,
self._controller.reached_start))
if self._controller.is_shutdown:
rospy.loginfo(
"{}: Controller shut itself down, setting success to false and premted"
.format(self._action_name))
success = False
self._as.set_preempted()
break
rospy.loginfo_throttle(
5, "publishing command: {}".format(self._controller.cmd))
self._vel_pub.publish(
ros_utils.cmd_to_JointVelocityMsg(self._controller.cmd))
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
#TODO there are other reasons to shutdown
self._controller.shutdown_controller()
rospy.loginfo('Recieved preemt request: %s: Preempted' %
self._action_name)
self._as.set_preempted()
success = False
break
# publish the feedback #TODO is it necissary?
# feedback = control_msgs.msg.FollowJointTrajectoryActionFeedback()
# feedback.feedback = self._feedback
# feedback.status = feedback.status.ACTIVE
# self._as.publish_feedback(feedback)
r.sleep()
if success:
#shutdown the controller and report success
self._controller.shutdown_controller()
self._result.error_code = control_msgs.msg.FollowJointTrajectoryResult.SUCCESSFUL
rospy.loginfo('%s: Succeeded' % self._action_name)
self._controller.is_shutdown = False
self._as.set_succeeded(self._result)
self._controller.is_shutdown = False
rospy.loginfo("Action server goal request complete")
def __init__(self, ID, task, methodType, demo, record):
"""
Setup of the ROS node. Publishing computed torques happens at 100Hz.
"""
# task type - table, laptop, or coffee task
self.task = task
# method type - A=IMPEDANCE, B=LEARNING
self.methodType = methodType
# optimal demo mode
if demo == "F" or demo == "f":
self.demo = False
elif demo == "T" or demo == "t":
self.demo = True
else:
print "Oopse - it is unclear if you want demo mode. Turning demo mode off."
self.demo = False
# record experimental data mode
if record == "F" or record == "f":
self.record = False
elif record == "T" or record == "t":
self.record = True
else:
print "Oopse - it is unclear if you want to record data. Not recording data."
self.record = False
# start admittance control mode
self.start_admittance_mode()
# ---- Trajectory Setup ---- #
# total time for trajectory
self.T = 15.0
# initialize trajectory weights
self.weights = 0
# if in demo mode, then set the weights to be optimal
if self.demo:
if self.task == TABLE_TASK or self.task == COFFEE_TASK:
self.weights = 1
elif self.task == LAPTOP_TASK or self.task == HUMAN_TASK:
self.weights = 10
# initialize start/goal based on task
if self.task == COFFEE_TASK or self.task == HUMAN_TASK:
pick = pick_shelf
else:
pick = pick_basic
if self.task == LAPTOP_TASK or self.task == HUMAN_TASK:
place = place_higher
else:
place = place_lower
start = np.array(pick) * (math.pi / 180.0)
goal = np.array(place) * (math.pi / 180.0)
self.start = start
self.goal = goal
# create the trajopt planner and plan from start to goal
self.planner = trajopt_planner.Planner(self.task)
self.planner.replan(self.start, self.goal, self.weights, 0.0, self.T,
0.5)
# save intermediate target position from degrees (default) to radians
self.target_pos = start.reshape((7, 1))
# save start configuration of arm
self.start_pos = start.reshape((7, 1))
# save final goal configuration
self.goal_pos = goal.reshape((7, 1))
# track if you have gotten to start/goal of path
self.reached_start = False
self.reached_goal = False
# keeps running time since beginning of program execution
self.process_start_T = time.time()
# keeps running time since beginning of path
self.path_start_T = None
# ----- Controller Setup ----- #
# stores maximum COMMANDED joint torques
self.max_cmd = MAX_CMD_TORQUE * np.eye(7)
# stores current COMMANDED joint torques
self.cmd = np.eye(7)
# stores current joint MEASURED joint torques
self.joint_torques = np.zeros((7, 1))
# P, I, D gains
p_gain = 50.0
i_gain = 0.0
d_gain = 20.0
self.P = p_gain * np.eye(7)
self.I = i_gain * np.eye(7)
self.D = d_gain * np.eye(7)
self.controller = pid.PID(self.P, self.I, self.D, 0, 0)
# ---- Experimental Utils ---- #
self.expUtil = experiment_utils.ExperimentUtils()
# store original trajectory
original_traj = self.planner.get_waypts_plan()
self.expUtil.update_original_traj(original_traj)
# ---- ROS Setup ---- #
rospy.init_node("pid_trajopt")
# create joint-velocity publisher
self.vel_pub = rospy.Publisher(prefix + '/in/joint_velocity',
kinova_msgs.msg.JointVelocity,
queue_size=1)
# create subscriber to joint_angles
rospy.Subscriber(prefix + '/out/joint_angles',
kinova_msgs.msg.JointAngles,
self.joint_angles_callback,
queue_size=1)
# create subscriber to joint_torques
rospy.Subscriber(prefix + '/out/joint_torques',
kinova_msgs.msg.JointTorque,
self.joint_torques_callback,
queue_size=1)
# 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 "----------------------------------"
# save experimental data (only if experiment started)
if self.record and self.reached_start:
print "Saving experimental data to file..."
weights_filename = "weights" + str(ID) + str(task) + methodType
force_filename = "force" + str(ID) + str(task) + methodType
tracked_filename = "tracked" + str(ID) + str(task) + methodType
original_filename = "original" + str(ID) + str(task) + methodType
deformed_filename = "deformed" + str(ID) + str(task) + methodType
self.expUtil.save_tauH(force_filename)
self.expUtil.save_tracked_traj(tracked_filename)
self.expUtil.save_original_traj(original_filename)
self.expUtil.save_deformed_traj(deformed_filename)
self.expUtil.save_weights(weights_filename)
# end admittance control mode
self.stop_admittance_mode()