def cb_loop(msg):
# Grab the time.
tloop = rospy.Time.now()
# Compute the nominal position/velocity based on the floating.
if (floating == FloatMode.ALL):
for i in range(dofs):
nompos[i] = msg.position[i]
nomvel[i] = msg.velocity[i]
elif (floating == FloatMode.ONE):
for i in range(dofs):
nomvel[i] = 0.0
nompos[joint] = msg.position[joint]
nomvel[joint] = msg.velocity[joint]
else:
t = (tloop - tspline).to_sec()
for i in range(dofs):
(nompos[i], nomvel[i]) = spline[i].calc(t)
# Populate the command message, adding gravity torques if available.
for i in range(dofs):
cmdmsg.position[i] = nompos[i]
cmdmsg.velocity[i] = nomvel[i]
try:
cmdmsg.effort = gravity.gravity(cmdmsg.position)
if (len(cmdmsg.effort) != dofs):
cmdmsg.effort = [0.0] * dofs
except:
cmdmsg.effort = [0.0] * dofs
# Overlay the triangle wave
if waving:
t = (tloop - twave).to_sec()
if (t <= 0.25 * waveperiod):
cmdmsg.position[wavejoint] += wavespeed * t
cmdmsg.velocity[wavejoint] += wavespeed
elif (t <= 0.75 * waveperiod):
cmdmsg.position[wavejoint] -= wavespeed * (t - 0.5 * waveperiod)
cmdmsg.velocity[wavejoint] -= wavespeed
else:
cmdmsg.position[wavejoint] += wavespeed * (t - waveperiod)
cmdmsg.velocity[wavejoint] += wavespeed
# If we have exceeded a period, shift and update.
if (t >= waveperiod):
wave_shift()
# Publish
cmdmsg.header.stamp = tloop
cmdpub.publish(cmdmsg)
rospy.init_node('tune')
# Define/check the robot joints.
hebi_define_robot(family, names)
# Create a publisher to send commands to the robot. Add some time
# for the subscriber to connect so we don't loose initial commands.
cmdpub = rospy.Publisher('/hebiros/robot/command/joint_state',
JointState,
queue_size=10)
rospy.sleep(0.4)
# Grab the initial position/velocity/effort/gravity.
(initpos, initvel, initeff) = get_joints()
try:
initgrav = gravity.gravity(initpos)
except:
initgrav = [0.0] * dofs
rospy.logwarn("No gravity model!")
if (len(initgrav) != dofs):
rospy.logerr("Gravity model returned incorrect number of joints!")
raise ValueError("Bad gravity model number of joints")
# Start commanding, ramping up the effort.
N = 50
for n in range(N):
cmdmsg.header.stamp = rospy.Time.now()
for i in range(dofs):
cmdmsg.position[i] = initpos[i]
cmdmsg.velocity[i] = 0.0
cmdmsg.effort[i] = initeff[i] + n * (
extract_size = 512
#extract_level=3; extract_size=1024
t0 = time.time()
print('start')
for sim in simlist:
frame = TL.loops[sim].target_frame
this_looper = TL.loops[sim]
ds = TL.loops[sim].load(frame)
print('make cg')
if 'cg' not in dir():
cg = ds.covering_grid(extract_level, [0.0] * 3, [extract_size] * 3)
rho = cg[YT_density]
else:
print('use old cg')
if 'ggg' not in dir():
ggg = gravity.gravity(rho, ds.parameters['GravitationalConstant'])
print('do solve')
ggg.solve()
print('done')
else:
print("WARNING old ggg")
if 1:
ms = trackage.mini_scrubber(this_looper.tr, core_id)
c = nar([ms.mean_x[-1], ms.mean_y[-1], ms.mean_z[-1]])
if 'rho_hack' not in dir():
print("On board the memory train")
rho_hack = rho + 0
x = cg[YT_x].v
y = cg[YT_y].v
Cap.label = 'Capacitor Charging' Cap.file = 'capacitor.html' physics.append(Cap) Induction = induction.induction(left, size, plot2d, msgwin) Induction.label = 'Electromagnetic Induction' physics.append(Induction) Trans = transformer.tran(left, size, plot2d, msgwin) Trans.label = 'Mutual Induction between coils' physics.append(Trans) Pendulum = pendulum.pend(left, size, plot2d, msgwin) Pendulum.label ='Pendulum Waveform' physics.append(Pendulum) Rodpend = rodpend.rodpend(left, size, plot2d, msgwin) Rodpend.label = 'Rod Pendulum measuring gravity' physics.append(Rodpend) Gravity = gravity.gravity(left, size, plot2d, msgwin) Gravity.label = 'Gravity by Time of Flight' physics.append(Gravity) Sound = usound.sound(left, size, plot2d, msgwin) Sound.label = 'Study Sound with 40KHz Piezo ' physics.append(Sound) Pt100 = pt100.pt100(left, size, plot2d, msgwin) Pt100.label = 'Temperature (PT100)' physics.append(Pt100) Rad = radiation.rad(left, size, plot2d, msgwin) Rad.label = 'Radiation Detection (MCA)' physics.append(Rad) Gm = gm.gm(left, size, plot2d, msgwin) Gm.label = 'GM Counter' physics.append(Gm)
fields=['density', YT_grav_energy])
if 0:
#projections
fig, ax = plt.subplots(1, 1)
ax.imshow(cg[YT_density].sum(axis=0).v)
fig.savefig('plots_to_sort/grav_20.png')
proj = ds.proj(YT_grav_energy, 0)
proj.to_pw().save('plots_to_sort/grav_21.png')
if 0:
cg.set_field_parameter('center', ds.arr([0.5] * 3, 'code_length'))
fig, ax = plt.subplots(1, 1)
plt.scatter(cg['radius'], cg[YT_potential_field])
fig.savefig('plots_to_sort/grav_22.png')
if 1:
import gravity
reload(gravity)
G = ds.parameters['GravitationalConstant'] #wants the 4pi
ggg = gravity.gravity(cg['density'].v, G)
ggg.solve()
fig, ax = plt.subplots(1, 1)
#ax.scatter(cg['radius'],ggg.phi)
#ax.scatter(cg['radius'],cg[YT_potential_field])
ax.scatter(ggg.phi, cg[YT_potential_field])
fig.savefig('plots_to_sort/grav_23.png')