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 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