def parse_objects(self, msg):
print(msg)
try:
json_msg = json.loads(msg)
except Exception:
print("something went wrong, message could be formatted wrong?")
return
if json_msg["error"] == 1:
print("something went wrong :/")
return
self.delete_objects()
obj_window = builder.get_object("obj-list")
data = json_msg["data"]
builder.get_object("ping-sensor").set_text("PING sensor: " + str(data["ping"]))
builder.get_object("ir-sensor").set_text("IR sensor: " + str(data["ir"]))
objects = data["objects"]
plotter = plot.Plotter()
index = 0
for obj in objects:
obj_window.add(gtk.Label("Object " + str(index)))
obj_window.add(gtk.Label("Distance: " + str(obj["distance"])))
obj_window.add(gtk.Label("Width: " + str(obj["width"])))
obj_window.add(gtk.Label("Angle: " + str(obj["angle"])))
plotter.add_object(obj["angle"], obj["distance"], obj["width"])
index += 1
plotter.draw("objects.png")
basewidth = 500
img = PIL.Image.open("objects.png")
wpercent = (basewidth / float(img.size[0]))
hsize = int((float(img.size[1]) * float(wpercent)))
img = img.resize((basewidth, hsize), PIL.Image.ANTIALIAS)
img.save("objects.png")
builder.get_object("image1").set_from_file("objects.png")
def __init__(self, p_gain, i_gain, d_gain, sim_flag):
"""
Setup of the ROS node. Publishing computed torques happens at 100Hz.
"""
# ---- ROS Setup ---- #
rospy.init_node("pid_vel_jaco")
# switch robot to torque-control mode if not in simulation
#if not sim_flag:
#self.init_torque_mode()
# create joint-torque 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)
# ---- Trajectory Setup ---- #
# list of waypoints along trajectory
waypts = [None]*len(traj)
for i in range(len(traj)):
waypts[i] = np.array(traj[i]).reshape((7,1))*(math.pi/180.0)
# scaling on speed
# 0.01 = normal speed
# 1.0 = slow
# 0.001 = fast
self.alpha = 0.005
# time for each (linear) segment of trajectory
deltaT = [2.0, 2.0]
# blending time (sec)
t_b = 1.0
# get trajectory planner
self.planner = planner.PathPlanner(waypts, t_b, deltaT, self.alpha)
# sets max execution time
self.T = self.planner.get_t_f()
# save intermediate target position from degrees (default) to radians
self.target_pos = waypts[0]
# save start configuration of arm
self.start_pos = waypts[0]
# save final goal configuration
self.goal_pos = waypts[-1]
# save current position of robot since last callback
self.curr_pos = self.start_pos
# track if you have gotten to start/goal of path
self.reached_start = False
self.reached_goal = False
# TODO THIS IS EXPERIMENTAL
self.interaction = False
self.max_torque = 30*np.eye(7)
# stores current COMMANDED joint torques
self.torque = np.eye(7)
# stores current joint MEASURED joint torques
self.joint_torques = np.zeros((7,1))
print "PID Gains: " + str(p_gain) + ", " + str(i_gain) + "," + str(d_gain)
self.p_gain = p_gain
self.i_gain = i_gain
self.d_gain = d_gain
# P, I, D gains
#P = self.p_gain*np.eye(7)
#I = self.i_gain*np.eye(7)
#D = self.d_gain*np.eye(7)
P = np.array([[50.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 50.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 50.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 50.5, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 50.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 50.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 50.0]])
I = self.i_gain*np.eye(7)
D = np.array([[20.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 50.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 20.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 50.5, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 20.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 20.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 20.0]])
self.controller = pid.PID(P,I,D,0,0)
# stuff for plotting
self.plotter = plot.Plotter(self.p_gain,self.i_gain,self.d_gain)
# 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
# publish to ROS at 1000hz
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(self.torque_to_JointVelocityMsg())
r.sleep()
# plot the error over time after finished
tot_path_time = time.time() - self.path_start_T
#self.plotter.plot_PID(tot_path_time)
self.plotter.plot_tau_PID(tot_path_time)
def __init__(self, k_gain, b_gain, j0, j1, j2, j3, j4, j5, j6):
rospy.init_node("impedence_jaco")
# create joint-torque publisher
self.torque_pub = rospy.Publisher(prefix + '/in/joint_torques',
kinova_msgs.msg.JointTorque,
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)
# convert target position from degrees (default) to radians
self.target_pos = np.array([j0, j1, j2, j3, j4, j5, j6]).reshape(
(7, 1)) * (math.pi / 180.0)
self.max_torque = 10 * np.eye(7)
self.torque = np.eye(7)
self.k_gain = k_gain
self.b_gain = b_gain
self.controller = pid.PID(P, I, D, 0, 0)
# stuff for plotting
self.plotter = plot.Plotter(self.p_gain, self.i_gain, self.d_gain)
# publish to ROS at 100hz
r = rospy.Rate(100)
while not rospy.is_shutdown(): #and nb != 'q':
#nb = getch.getche() # need to import
self.torque_pub.publish(self.torque_to_JointTorqueMsg())
r.sleep()
# plot the error over time after finished
#self.plotter.plot_Impedence()
def velocity_to_JointTorqueMsg(self):
"""
Returns a JointTorque Kinova msg from an array of torques
"""
jointCmd = kinova_msgs.msg.JointTorque()
jointCmd.joint1 = self.torque[0][0]
jointCmd.joint2 = self.torque[1][1]
jointCmd.joint3 = self.torque[2][2]
jointCmd.joint4 = self.torque[3][3]
jointCmd.joint5 = self.torque[4][4]
jointCmd.joint6 = self.torque[5][5]
jointCmd.joint7 = self.torque[6][6]
return jointCmd
def Impedence_control(self, pos):
"""
Return a control torque based on Impedence control
"""
# deal with angle wraparound when computing difference
error = -((self.target_pos - pos + math.pi) %
(2 * math.pi) - math.pi)
return -self.controller.update_Impedence(error)
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
pos_curr = np.array([
msg.joint1, msg.joint2, msg.joint3, msg.joint4, msg.joint5,
msg.joint6, msg.joint7
]).reshape((7, 1))
# convert to radians
pos_curr = pos_curr * (math.pi / 180.0)
# update torque from PID based on current position
self.torque = self.Impedence_control(pos_curr)
def graphp2(fill_id, display_tz):
conn = util.getConn()
general = util.getRowFromTableById('general_config', 1, conn=conn)
query = []
options = {}
options['axisColors'] = [['#FF0000', '#0000FF']]
options['axisLabels'] = [['Temperature (F)', 'Humidity (%)']]
options['dataAxisMap'] = [[0, 0, 1, 1]]
options['minMaxAreas'] = [[1, 1, 1, 1]]
fill = util.getRowFromTableById("fill", fill_id, conn=conn)
# bin = util.getRowFromTableById('bin', fill['bin_id'], conn=conn)
# topSection = util.getRowFromTableById("bin_section", TOP_BIN_SECTION, conn=conn)
# bottomSection = util.getRowFromTableById("bin_section", BOTTOM_BIN_SECTION, conn=conn)
begin_datetime = fill["air_begin_datetime"]
if not begin_datetime:
abort(404, 'Fill has not started air.')
if not fill["air_end_datetime"]:
end_datetime = util.getDateFromParam("now")
else:
end_datetime = fill["air_end_datetime"]
if general['inletoutlet']:
query.append(['inlet', [TEMP_READ_TYPE_ID]])
query.append(['outlet', [TEMP_READ_TYPE_ID]])
query.append(['inlet', [HUM_READ_TYPE_ID]])
query.append(['outlet', [HUM_READ_TYPE_ID]])
options['seriesLabels'] = [[
'inlet temp', 'outlet temp', 'inlet RH', 'outlet RH'
]]
else:
query.append(['sensor', [TOP_BIN_SECTION, TEMP_READ_TYPE_ID]])
query.append(['sensor', [BOTTOM_BIN_SECTION, TEMP_READ_TYPE_ID]])
query.append(['sensor', [TOP_BIN_SECTION, HUM_READ_TYPE_ID]])
query.append(['sensor', [BOTTOM_BIN_SECTION, HUM_READ_TYPE_ID]])
options['seriesLabels'] = [[
'top temp', 'bottom temp', 'top RH', 'bottom RH'
]]
if general['emchrs_per_point']:
query.append(['maxtemp', []])
options['seriesLabels'][0].append('target temp')
options['dataAxisMap'][0].append(0)
options['minMaxAreas'][0].append(0)
# 600 x 400 was size of old graph
graph_dims = (600, 400)
#Auto sample period
sample_period = get_best_sample_period(graph_dims[0] / 5,
begin_datetime,
end_datetime,
conn=conn)
logging.debug("Sample period: " + str(sample_period))
#samplePeriod=120
our_graph_data = graph_data.graph_data_struct(fill['bin_id'], query,
sample_period,
begin_datetime, end_datetime)
p = plot.Plotter(our_graph_data, None, options, graph_dims, display_tz)
return p.getPNGBuffer()
graph_data_top = graph_data_bottom
graph_data_bottom = None
graphq_switch_options(options)
for g in range(2):
if not 0 in options['dataAxisMap'][g]:
graphq_switch_axis(options, g)
#degree symbol fix
for g in range(len(options['axisLabels'])):
for idx in range(len(options['axisLabels'][g])):
options['axisLabels'][g][idx] = options['axisLabels'][g][
idx].replace('°', '$^\circ$')
logging.debug("options=" + str(options))
p = plot.Plotter(graph_data_top, graph_data_bottom, options, graph_dims,
display_tz)
response.content_type = "image/png"
return p.getPNGBuffer().getvalue()
def graphq_switch_options(options):
options['axisColors'][0] = options['axisColors'][1]
options['axisLabels'][0] = options['axisLabels'][1]
options['dataAxisMap'][0] = options['dataAxisMap'][1]
options['minMaxAreas'][0] = options['minMaxAreas'][1]
options['seriesLabels'][0] = options['seriesLabels'][1]
def graphq_switch_axis(options, g):
options['axisColors'][g][0] = options['axisColors'][g][1]
options['axisLabels'][g][0] = options['axisLabels'][g][1]
def __init__(self, p_gain, i_gain, d_gain, sim_flag):
"""
Setup of the ROS node. Publishing computed torques happens at 100Hz.
"""
# ---- ROS Setup ---- #
rospy.init_node("pid_torque_jaco")
# switch robot to torque-control mode if not in simulation
if not sim_flag:
self.init_torque_mode()
# create joint-torque publisher
self.torque_pub = rospy.Publisher(prefix + '/in/joint_torque',
kinova_msgs.msg.JointTorque,
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)
# create subscriber to joint_state
rospy.Subscriber(prefix + '/out/joint_state',
sensor_msgs.msg.JointState,
self.joint_state_callback,
queue_size=1)
# ---- Trajectory Setup ---- #
# list of waypoints along trajectory
waypts = [None] * len(traj)
for i in range(len(traj)):
waypts[i] = np.array(traj[i]).reshape((7, 1)) * (math.pi / 180.0)
# scaling on speed
# 1.5 = normal speed
# 2.5 = slow
# 1.0 = fast
self.alpha = 1.0
# time for each (linear) segment of trajectory
deltaT = [2.0, 2.0]
# blending time (sec)
t_b = 1.0
# get trajectory planner
self.planner = planner.PathPlanner(waypts, t_b, deltaT, self.alpha)
# sets max execution time
self.T = self.planner.get_t_f()
# save intermediate target position from degrees (default) to radians
self.target_pos = waypts[0]
# save start configuration of arm
self.start_pos = waypts[0]
# save final goal configuration
self.goal_pos = waypts[-1]
# save current position of robot since last callback
self.curr_pos = self.start_pos
# track if you have gotten to start/goal of path
self.reached_start = False
self.reached_goal = False
# TODO THIS IS EXPERIMENTAL
self.interaction = False
self.max_torque = 30 * np.eye(7)
# stores current COMMANDED joint torques
self.torque = np.eye(7)
# stores current joint MEASURED joint torques
self.joint_torques = np.zeros((7, 1))
print "PID Gains: " + str(p_gain) + ", " + str(i_gain) + "," + str(
d_gain)
self.p_gain = p_gain
self.i_gain = i_gain
self.d_gain = d_gain
# P, I, D gains
#P = self.p_gain*np.eye(7)
#I = self.i_gain*np.eye(7)
#D = self.d_gain*np.eye(7)
self.P = np.array([[40.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 30.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 40.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 50.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 20.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 15.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0]])
self.I = self.i_gain * np.eye(7)
self.D = np.array([[10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 7.5, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 5.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]])
self.controller = pid.PID(self.P, self.I, self.D, 0, 0)
# stuff for plotting
self.plotter = plot.Plotter(self.p_gain, self.i_gain, self.d_gain)
# 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
# publish to ROS at 100hz
#TODO Torque control doesn't work unless you are running at 1000Hz at least (but not sure if works)
#TODO Noticed weird behavior where torque control dies silently (motors lock up and dont move)
# if the Hz rate is > 100. :( Need to debug
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.torque_pub.publish(self.torque_to_JointTorqueMsg())
r.sleep()
# plot the error over time after finished
tot_path_time = time.time() - self.path_start_T
#self.plotter.plot_PID(tot_path_time)
self.plotter.plot_tau_PID(tot_path_time)
# switch back to position control after finished if not in simulation
if not sim_flag:
service_address = prefix + '/in/set_torque_control_mode'
rospy.wait_for_service(service_address)
try:
switchTorquemode = rospy.ServiceProxy(service_address,
SetTorqueControlMode)
switchTorquemode(0)
return None
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# Split system function to access components
U = dol.Function(FS)
v, T = dol.split(U)
# Initial conditions
U_n = dol.interpolate(dol.Expression(("5*(1-x[0])","2-exp(x[0])"), degree = 2), FS)
v_n, T_n = dol.split(U_n)
dx, grad, derivative, solve = dol.dx, dol.grad, dol.derivative, dol.solve
# Define variational problem
F = ((v - v_n) / k)*f_1*dx + epsilon_1*v*f_1*dx + kappa*grad(T)[0]*f_1*dx +\
A_1hat*((T - T_n) / k)*f_2*dx + a_T*grad(T)[0]*v*f_2*dx + M_s1*grad(v)[0]*f_2*dx - Q*f_2*dx
L = kappa*f_1*dol.ds+M_s1*5*f_2*dol.ds
# Solve the system for each time step
t = 0
pltr_T = plot.Plotter(mesh, id_ = '')
pltr_vel = plot.Plotter(mesh, id_ = '')
temp = lambda y: T_n([y])
vel = lambda y: v_n([y])
for n in range(num_steps):
t += dt
if n%1 == 0:
pltr_vel.plot(vel,'qtcm1/velocity/', n, t, quantity ='line2_vel')
pltr_T.plot(temp,'qtcm1/velocity/', n, t, quantity ='line2_temp')
# Solve variational problem for time step
J = derivative(F, U)
solve(F == 0, U, bcs, J = J)
U_n.assign(U)
pltr_vel.create_video()
pltr_T.create_video()