def publishJointStates(side, lcm, status):
global l_names
global r_names
state = joint_state_t()
state.utime = status.utime
state.num_joints = 11
state.joint_velocity = [0]*11
state.joint_effort = [0]*11
state.joint_position = [0]*11
state.joint_position[0] = get_mapping(status.positionA)
state.joint_position[1] = get_mapping(status.positionA)
state.joint_position[2] = get_mapping(status.positionA)
state.joint_position[3] = get_mapping(status.positionB)
state.joint_position[4] = get_mapping(status.positionB)
state.joint_position[5] = get_mapping(status.positionB)
state.joint_position[6] = get_mapping(status.positionC)
state.joint_position[7] = get_mapping(status.positionC)
state.joint_position[8] = get_mapping(status.positionC)
state.joint_position[9] = -0.002 * (status.positionS-137)
state.joint_position[10] = 0.002 * (status.positionS-137)
if (side.upper()[0] == 'R'):
state.joint_name = r_names
lcm.publish("ROBOTIQ_" + side.upper() + "_STATE", state.encode())
else:
state.joint_name = l_names
lcm.publish("ROBOTIQ_" + side.upper() + "_STATE", state.encode())
def conHandler(channel, data):
global count
global X0
global u
global xytheta
global theta
#global A
#global B
global zreal
#global l0
#global Dt
print "message received on channel: %s" % channel
msg = positionSim_t.decode(data)
u = matrix(msg.u).H #[2]
#print "u looks like:"
#print u
#print "norm: %s"%linalg.norm(u)
dxytheta = Dt * matrix([[float(cos(theta)), 0], [float(sin(theta)), 0],
[0, 1]]) * u
#print dxytheta
#chg = sqrt(dxytheta[0]**2 + dxytheta[1]**2)
#print chg
#if chg < 0.01:
# count = count +1
# if count > 5:
# print "move in"
# count = count - 2
#dxytheta[0] = -0.2
# print theta
# dxytheta[2] = dxytheta[2] -0.5
# print "ADJUSTING U!!\n\n\n\n"
#dxytheta[0] = -0.01
#u[0] = -3
#u[1] = -3
xytheta[0] = xytheta[0] + dxytheta[1]
xytheta[1] = xytheta[1] + dxytheta[0]
xytheta[2] = xytheta[2] + dxytheta[2]
#%control law ends here
if xytheta[2] > 2 * pi:
xytheta[2] = xytheta[2] - 2 * pi
elif xytheta[2] < 0:
xytheta[2] = xytheta[2] + 2 * pi
X0[1] = xytheta[0]
X0[0] = xytheta[1]
theta = xytheta[2]
retMsg = positionSim_t()
retMsg.X0 = X0 #make X0 three dimensional to include angle information
retMsg.theta = theta #Should be changed with 3 dimensional X0 containing theta
lcm.publish("conReturn", retMsg.encode())
def retrieve(channel, data):
global xi, yi
#print "retrieve"
#global trap
plum.movePuffs()
dx = 1
dy = dx
msg = positionSim_t.decode(data)
x = msg.X0[1]
y = msg.X0[0]
xi = x
yi = y
print x, y
#print x,y
normalizingValue = 10.0 # just trying to bring the puff count to a reasonable
#value. it looks like the concentration of like 0.3 is reasonable.
c = plum.concentration(x, y, dx) / normalizingValue
x1 = plum.concentration(x + dx, y, dx) / normalizingValue
x2 = plum.concentration(x - dx, y, dx) / normalizingValue
x3 = plum.concentration(x, y - dx, dx) / normalizingValue
x4 = plum.concentration(x, y - dx, dx) / normalizingValue
vy, vx = plum.flow.getVector(x, y)
#print "return the concentration at (%s, %s)" %(x, y)
#print x1, x2, x3, x4, vx, vy, c
DU_dy0 = (vy >= 0) * (x1 - c) / (dx) + (vy < 0) * (c - x2) / (dx)
DU_dx0 = (vx >= 0) * (x3 - c) / (dx) + (vx < 0) * (c - x4) / (dx)
DU = (DU_dx0, DU_dy0)
DU_p = (-DU_dy0, DU_dx0)
DU_p = DU_p / LA.norm(DU_p + np.finfo(float).eps)
V0 = (vx, vy)
D2U0 = (x3 + x4 - 2 * c) / dx**2 + (x1 + x2 - 2 * c) / dy**2
U0 = c
print "DU: %s DU_p: %s\nV0: %s D2U0: %s\nU0: %s" % (DU, DU_p, V0, D2U0,
U0)
retMsg = positionSim_t()
retMsg.DU = DU
retMsg.DU_p = DU_p
retMsg.V0 = V0
retMsg.D2U0 = D2U0
retMsg.U0 = U0
lcm.publish("dataReturn", retMsg.encode())
def publishSystemStatus(side, lcm, status):
global connectPublished
global activePublished
if connectPublished and activePublished:
return
msg = drc.system_status_t()
msg.utime = (time() * 1000000)
msg.system = 4 #provided as the system level for grippers
msg.importance = 0
msg.frequency = 0
if connectPublished:
if status and status.activated == 1:
msg.value = side.upper() + " ROBOTIQ HAND ACTIVE: Receiving status and active"
lcm.publish("SYSTEM_STATUS", msg.encode())
activePublished = True
else:
if status:
msg.value = side.upper() + " ROBOTIQ HAND ALIVE: Receiving status messages"
lcm.publish("SYSTEM_STATUS", msg.encode())
connectPublished = True
parser.add_option(
'-s',
action="store_false",
dest="extended",
help="If provided, use 64-length descriptor instead of 128",
default=True)
parser.add_option('-n',
type="int",
dest="nOctaves",
help="Number of octaves",
default=4)
parser.add_option('-l',
type='int',
dest="nOctaveLayers",
help="Number of octave layers",
default=2)
(options, args) = parser.parse_args()
#Populate the lcm message
msg = SurfParams()
msg.extended = options.extended
msg.hessianThreshold = options.hessianThreshold
msg.nOctaves = options.nOctaves
msg.nOctaveLayers = options.nOctaveLayers
msg.utime = int(time.time() * 1000000)
#Publish the lcm message
lcm = lcm.LCM()
lcm.publish(SURFGPU_PARAMS, msg.encode())
def conHandler(channel, data):
global count
global u
global X0
global Xhat
print "message received on channel: %s" % channel
msg = positionSim_t.decode(data)
c = msg.con
V0 = matrix(msg.V0).H #[2]
DU_dx0, DU_dy0, D2U0 = gradientDivergence(c[0], c[1], c[2], c[3], c[4],
V0[0], V0[1])
DU = asmatrix([float(DU_dx0), float(DU_dy0)]).H
DU_p = asmatrix([float(-DU_dy0), float(DU_dx0)]).H
U0 = c[0]
#print DU
X0_rec = matrix(msg.X0).H
theta = matrix(msg.theta).H
dotXhat_1 = float( -( (V0.H * DU) + k1 *D2U0 + k2 * U0 ) ) \
* divide(DU, (LA.norm(DU)**2))
dotXhat_1 = nanCheck(dotXhat_1)
dotXhat_2 = -DU * (DU.H * (Xhat[:, 0] - X0[:, 0]) + U0 - threshold)
dotXhat_2 = nanCheck(dotXhat_2)
dotXhat_2 = k3 * dotXhat_2 / LA.norm(dotXhat_2)
dotXhat_2 = nanCheck(dotXhat_2)
dotXhat_3 = c1 * DU_p
dotXhat = dotXhat_1 + dotXhat_2 + dotXhat_3
Xhatdot = dotXhat
#this is the control
V0_robot = -c_r * (X0 - Xhat) + v_compensate * Xhatdot
Xhat = Xhat + (Dt * Xhatdot) #mysaturation(Xhatdot, Xhatdot_max)).H
X0 = X0 + (Dt * V0_robot) #mysaturation(V0_robot, V0_robot_max)).H
Xhat_diff = (Dt * mysaturation(Xhatdot, Xhatdot_max)).H
X0_diff = (Dt * mysaturation(V0_robot, V0_robot_max)).H
vd[0] = X0_diff[0] / Dt
vd[1] = X0_diff[1] / Dt
Dinv = matrix([ [float(cos(theta)), float(-l0*sin(theta))], \
[float(sin(theta)), float(l0*cos(theta))]])
D = matrix([ [float(cos(theta)), float(sin(theta))], \
[float(-sin(theta)/l0), float(cos(theta)/l0)]])
#Control law part generates the control input corresponding to u in equation 3 and 4
F = LA.inv(B) * (-A * u - c0 * (u - D * vd))
du = (A * u + B * F) * Dt
u = du + u
#print du
#print "norm du %s con: %s"%(linalg.norm(du), c)
#print "flow vector: %s"%V0
#if linalg.norm(du) < 0.2 and c[0] < 0.005:
# print "start turning around\n\n\n\n\n\n\n\n\n"
# print du
#print "help"
retMsg = positionSim_t()
retMsg.u = u
lcm.publish("controlReturn", retMsg.encode())
if len(args):
readFromCmdline(args)
lcm = lcm.LCM("udpm://239.255.76.67:7667?ttl=%d"%(ttl))
msg = route2_tree_t()
msg.timestamp = int(time.time() * 1000000)
print rtree
msg.n = len(rtree)
rtables=list()
for (node, rtable) in rtree:
rt = route2_table_t()
rt.node = node
rt.n = len(rtable)
entries=list()
for (dest,nh,w) in rtable:
re = route2_entry_t()
re.dest = dest
re.node = nh
re.weight = w
entries.append(re)
rt.entries = entries
rtables.append(rt)
msg.rtable = rtables
#print msg
print "publishing msg to ",options.channel
lcm.publish(options.channel, msg.encode())
readFile(options.filename)
if len(args):
readFromCmdline(args)
lcm = lcm.LCM("udpm://239.255.76.67:7667?ttl=1")
msg = route_tree_t()
msg.timestamp = int(time.time() * 1000000)
#msg.timestamp = 0
print rtree
msg.n = len(rtree)
rtables=list()
for (node, rtable) in rtree:
rt = route_table_t()
rt.node = node
rt.n = len(rtable)
entries=list()
for (nh,w) in rtable:
re = route_entry_t()
re.node = nh
re.weight = w
entries.append(re)
rt.entries = entries
rtables.append(rt)
msg.rtable = rtables
#print msg
lcm.publish("RNP", msg.encode())
%(msg.DU, msg.DU_p, msg.V0, msg.D2U0, msg.U0)
print msg.DU[1]
def envReturnHandler(channel, data):
pass
lcm = lcm.LCM()
subEnv = lcm.subscribe("dataReturn", envHandler)
subEnvRet = lcm.subscribe("envUpdateConfirm", envReturnHandler)
param = auxiliary.Parameters()
ts = param.T
Dt = param.dt
T_thresh = 2 * (1 / Dt)
dummyMsg = positionSim_t()
dummyMsg.X0[0] = 12
dummyMsg.X0[1] = 25
#for T in linspace(0, ts, (1/Dt) *ts): #
print ts / Dt
for T in xrange(int(ts / Dt)):
print T
if T >= T_thresh:
dummyMsg.T = T
lcm.publish("envRetrieve", dummyMsg.encode())
lcm.handle() #envReturn
#!/usr/bin/env python
import time
from optparse import OptionParser
import perls
import lcm
SIFTGPU_PARAMS = "SIFTGPU_PARAMS"
SiftParams = perls.lcmtypes.perllcm.van_siftgpu_params_t
if __name__ == '__main__':
msg = SiftParams()
#Sending these parameters seems to have an effect for CUDA-enabled systems.
# NOTE: If siftgpu was compiled without CUDA support, these values do nothing
msg.dogThreshold = 0.02
msg.edgeThreshold = 10.0
msg.dogLevels = 3
msg.filterWidthFactor = 4.0
#...these parameters do not seem to have an effect even on CUDA-enabled systems.
msg.orientWindowFactor = 2.0
msg.gridSizeFactor = 3.0
lcm = lcm.LCM()
lcm.publish(SIFTGPU_PARAMS, msg.encode())
def confirmFinish():
msg = positionSim_t()
lcm.publish("envUpdateConfirm", msg.encode())
