"""LCM type definitions
This file automatically generated by lcm.
DO NOT MODIFY BY HAND!!!!
"""


from io import BytesIO
import struct

class lcmt_iiwa_status(object):

    __slots__ = ["utime", "num_joints", "joint_position_measured", "joint_velocity_estimated", "joint_position_commanded", "joint_position_ipo", "joint_torque_measured", "joint_torque_commanded", "joint_torque_external"]

    __typenames__ = ["int64_t", "int32_t", "double", "double", "double", "double", "double", "double", "double"]

    __dimensions__ = [None, None, ["num_joints"], ["num_joints"], ["num_joints"], ["num_joints"], ["num_joints"], ["num_joints"], ["num_joints"]]

    def __init__(self):
        self.utime = 0
        """
        The timestamp in microseconds.
        LCM Type: int64_t
        """

        self.num_joints = 0
        """ LCM Type: int32_t """
        self.joint_position_measured = []
        """
        From FRI documentation: "The currently measured joint positions of the
        robot in radians."
        LCM Type: double[num_joints]
        """

        self.joint_velocity_estimated = []
        """
        The FRI driver does not provide velocity; we estimate it in our driver via
        a low-pass filter.  Units are radians / sec.
        LCM Type: double[num_joints]
        """

        self.joint_position_commanded = []
        """
        From FRI documentation: "The last commanded joint positions of the robot in
        radians."
        LCM Type: double[num_joints]
        """

        self.joint_position_ipo = []
        """
        From FRI documentation:
        "The joint positions commanded by the interpolator in radians. When
        commanding a motion overlay in your robot application, this method will
        give access to the joint positions currently commanded by the motion
        interpolator.  This method will return NULL during monitoring mode."
        
        The Kuka motion interpolated code is a black-box to us, so we typically do
        not try to model/simulate this signal.
        LCM Type: double[num_joints]
        """

        self.joint_torque_measured = []
        """
        From FRI documentation: "The currently measured joint torques of the robot
        in Nm."
        
        This appears to be the raw measurement of the torque sensors, which is
        attempting to track joint_torque_commanded.
        
        This is *negative* w.r.t. the conventions outlined in manipulation/README.
        LCM Type: double[num_joints]
        """

        self.joint_torque_commanded = []
        """
        From FRI documentation: "The last commanded joint torques of the robot in
        Nm."
        
        This appears to be most similar to the torque input to multibody plant.
        This is *negative* w.r.t. the conventions outlined in manipulation/README.
        LCM Type: double[num_joints]
        """

        self.joint_torque_external = []
        """
        From FRI documentation: "The currently measured external joint torques of
        the robot in Nm.  The external torques corresponds to the measured torques
        when removing the torques induced by the robot itself."
        
        This appears to be the contact forces (in joint coordinates) as well as any
        residuals from modeling errors (as computed by the onboard Kuka inverse
        dynamics model, which is a black-box to us). Recall that the inertia of
        the tool is included (potentially very approximately) in the onboard Kuka
        model, so long as a tool is defined in the active Sunrise project. You can
        use the Kuka pendant to teach the tool inertia.
        
        This is correct w.r.t. the conventions outlined in manipulation/README.
        LCM Type: double[num_joints]
        """


    def encode(self):
        buf = BytesIO()
        buf.write(lcmt_iiwa_status._get_packed_fingerprint())
        self._encode_one(buf)
        return buf.getvalue()

    def _encode_one(self, buf):
        buf.write(struct.pack(">qi", self.utime, self.num_joints))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_position_measured[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_velocity_estimated[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_position_commanded[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_position_ipo[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_torque_measured[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_torque_commanded[:self.num_joints]))
        buf.write(struct.pack('>%dd' % self.num_joints, *self.joint_torque_external[:self.num_joints]))

    @staticmethod
    def decode(data: bytes):
        if hasattr(data, 'read'):
            buf = data
        else:
            buf = BytesIO(data)
        if buf.read(8) != lcmt_iiwa_status._get_packed_fingerprint():
            raise ValueError("Decode error")
        return lcmt_iiwa_status._decode_one(buf)

    @staticmethod
    def _decode_one(buf):
        self = lcmt_iiwa_status()
        self.utime, self.num_joints = struct.unpack(">qi", buf.read(12))
        self.joint_position_measured = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_velocity_estimated = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_position_commanded = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_position_ipo = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_torque_measured = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_torque_commanded = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        self.joint_torque_external = struct.unpack('>%dd' % self.num_joints, buf.read(self.num_joints * 8))
        return self

    @staticmethod
    def _get_hash_recursive(parents):
        if lcmt_iiwa_status in parents: return 0
        tmphash = (0xf8b7dc0214255e51) & 0xffffffffffffffff
        tmphash  = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff
        return tmphash
    _packed_fingerprint = None

    @staticmethod
    def _get_packed_fingerprint():
        if lcmt_iiwa_status._packed_fingerprint is None:
            lcmt_iiwa_status._packed_fingerprint = struct.pack(">Q", lcmt_iiwa_status._get_hash_recursive([]))
        return lcmt_iiwa_status._packed_fingerprint

    def get_hash(self):
        """Get the LCM hash of the struct"""
        return struct.unpack(">Q", lcmt_iiwa_status._get_packed_fingerprint())[0]

