def joint_cmd_handler(self,channel,data):
msgIn = drc.robot_plan_t.decode(data)
msgOut = abblcm.abb_irb140joints()
msgOut.utime = msgIn.utime
msgOut.pos = [msgIn.plan[-1].joint_position[j]/math.pi*180.0 for j in range(msgIn.plan[-1].num_joints)]
msgOut.vel = [msgIn.plan[-1].joint_velocity[j]/math.pi*180.0 for j in range(msgIn.plan[-1].num_joints)]
#msgOut.pos[0] = -msgOut.pos[0]
#msgOut.pos[2] = -msgOut.pos[2]
#msgOut.vel[0] = -msgOut.vel[0]
#msgOut.vel[2] = -msgOut.vel[2]
self.lc.publish("IRB140JOINTCMD",msgOut.encode())
def joint_plan_handler(self,channel,data):
msgIn = drc.robot_plan_t.decode(data)
msgOut = abblcm.abb_irb140joint_plan()
msgOut.utime = msgIn.utime
msgOut.n_cmd_times = msgIn.num_states
msgOut.joint_cmd = [abblcm.abb_irb140joints() for i in range(msgOut.n_cmd_times)]
for i in range(msgOut.n_cmd_times):
msgOut.joint_cmd[i].pos = [msgIn.plan[i].joint_position[j]/math.pi*180.0 for j in range(msgIn.plan[i].num_joints)]
msgOut.joint_cmd[i].vel = [msgIn.plan[i].joint_velocity[j]/math.pi*180.0 for j in range(msgIn.plan[i].num_joints)]
#msgOut.joint_cmd[i].pos[0] = -msgOut.joint_cmd[i].pos[0]
#msgOut.joint_cmd[i].pos[2] = -msgOut.joint_cmd[i].pos[2]
#msgOut.joint_cmd[i].vel[0] = -msgOut.joint_cmd[i].vel[0]
#msgOut.joint_cmd[i].vel[2] = -msgOut.joint_cmd[i].vel[2]
self.lc.publish("IRB140JOINTPLAN",msgOut.encode())