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