def listen_blender():
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.bind(('0.0.0.0', 8019))
sock.listen(5)
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
conn, addr = sock.accept()
while True:
try:
# conn, addr = sock.accept()
rawData = conn.recv(1024).decode()
data = json.loads(rawData)
# Send data to onboard rover
msg = ArmPosition()
msg.joint_a = data['A']
msg.joint_b = data['B']
msg.joint_c = data['C']
msg.joint_d = data['D']
msg.joint_e = data['E']
lcm_.publish('/ik_ra_control', msg.encode())
except socket.error as exc:
# print(exc)
pass
async def listen_blender():
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.bind(('0.0.0.0', 8019))
sock.listen(5)
sock.settimeout(0.1)
conn = None
while conn is None:
try:
conn, addr = sock.accept()
except socket.timeout:
await asyncio.sleep(0.1)
while True:
try:
rawData = str(conn.recv(1024)).replace('\'', '"')
rawData = rawData[2:len(rawData) - 1]
data = json.loads(rawData)
# Send data to onboard rover
msg = ArmPosition()
msg.joint_a = data['A']
msg.joint_b = data['B']
msg.joint_c = data['C']
msg.joint_d = data['D']
msg.joint_e = data['E']
lcm_.publish('/ik_ra_control', msg.encode())
except socket.timeout:
await asyncio.sleep(0.1)
def publish_config(self, config, channel):
arm_position = ArmPosition()
arm_position.joint_a = config[0]
arm_position.joint_b = config[1]
arm_position.joint_c = config[2]
arm_position.joint_d = config[3]
arm_position.joint_e = config[4]
self.lcm_.publish(channel, arm_position.encode())
def preview(self):
logger.info('Previewing')
preview_robot = copy.deepcopy(self.state)
num_steps = 500
t = 0
print(self.current_spline(1))
while t < 1:
target = self.current_spline(t)
target_pos = ArmPosition()
target_pos.joint_a = target[0]
target_pos.joint_b = target[1]
target_pos.joint_c = target[2]
target_pos.joint_d = target[3]
target_pos.joint_e = -target[4]
print(target)
preview_robot.set_angles(target_pos)
self.solver.FK(preview_robot)
print(preview_robot.angles)
self.publish_transforms(preview_robot)
print(t)
time.sleep(0.002)
t += 1 / num_steps
path_message = DebugMessage()
path_message.isError = False
path_message.message = 'Preview Done'
self.lcm_.publish('/debugMessage', path_message.encode())
def arm_position_callback(self, channel, msg):
"""
Handler for angles
Triggers forward kinematics
"""
if self.ik_enabled and not self.enable_execute:
return
arm_position = ArmPosition.decode(msg)
if channel == "/arm_position":
self.state.set_angles(arm_position)
self.solver.FK(self.state)
self.publish_transforms(self.state)
def ik_ra_control_callback(channel, msg):
ircc_constant = 360
input_data = ArmPosition.decode(msg)
# Wait until hardware implements
input_data.joint_a /= ircc_constant
# set_demand(input_data.joint_a, talon_srx.TalonControlMode.kPositionMode)
input_data.joint_b /= ircc_constant
# set_demand(input_data.joint_b, talon_srx.TalonControlMode.kPositionMode)
input_data.joint_c /= ircc_constant
# set_demand(input_data.joint_c, talon_srx.TalonControlMode.kPositionMode)
input_data.joint_d /= ircc_constant
# set_demand(input_data.joint_d, talon_srx.TalonControlMode.kPositionMode)
input_data.joint_e /= ircc_constant
def listen_blender():
global ik_mode
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
sock.bind(('0.0.0.0', 8019))
sock.listen(5)
conn, addr = sock.accept()
while True:
try:
# conn, addr = sock.accept()
rawData = conn.recv(1024).decode()
try:
data = json.loads(rawData)
except:
continue
diff_count = 0
if prev_messages[0] is None:
diff_count = NUM_PREV_MESSAGES
else:
for pd in prev_messages:
for i in range(5):
j = chr(ord('A') + i)
if pd is None or prev_messages[0][j] != pd[j]:
diff_count += 1
break
prev_messages.pop(0)
prev_messages.append(data)
if diff_count <= 1:
continue
# Send data to onboard rover
msg = ArmPosition()
msg.joint_a = -data['A']
msg.joint_b = -data['B']
msg.joint_c = -data['C']
msg.joint_d = -data['D']
msg.joint_e = -data['E']
lcm_.publish('/ik_ra_control', msg.encode())
except socket.error as exc:
print(exc)
pass
def cartesian_control_callback(self, channel, msg):
if self.enable_execute:
return
cart_msg = IkArmControl.decode(msg)
delta = [cart_msg.deltaX, cart_msg.deltaY, cart_msg.deltaZ]
joint_angles, is_safe = self.solver.IK_delta(delta, 3)
if is_safe:
arm_position = ArmPosition()
arm_position.joint_a = joint_angles["joint_a"]
arm_position.joint_b = joint_angles["joint_b"]
arm_position.joint_c = joint_angles["joint_c"]
arm_position.joint_d = joint_angles["joint_d"]
arm_position.joint_e = -joint_angles["joint_e"]
self.state.set_angles(arm_position)
self.solver.FK(self.state)
self.publish_transforms(self.state)
if self.sim_mode:
self.lcm_.publish('/arm_position', arm_position.encode())
else:
self.lcm_.publish('/ik_ra_control', arm_position.encode())
def fk_callback(channel, msg):
if ik_mode:
return
angles = ArmPosition.decode(msg)
data = {
"type": "FK",
"joint_a": angles.joint_a,
"joint_b": angles.joint_b,
"joint_c": angles.joint_c,
"joint_d": angles.joint_d,
"joint_e": angles.joint_e,
}
try:
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
if VAGRANT:
sock.connect(("10.0.2.2", 8018))
else:
sock.connect(("127.0.0.1", 8018))
sock.send(json.dumps(data).encode())
except socket.error as exc:
# print(exc)
pass