Invert rigid body transformation matrix

----------Table-driven crc function----------***/ Inputs: -size of the character array, the CRC of which is being computed */ - the initial value of

* Empties the serial buffer and the entire message queue. * Optionally, the queue may be emptied into a provided list. * @param queue If not null, the

* Checks the consistency of this message. * @param whyNot Optionally, a reason for validation failure will be * written here. * @p

* Configure this Transport for communication. * If this Transport is already configured, it will be closed and reconfigured. * The RX buffer and Messa

* Get a pointer to the payload withing this Message's internal storage. * @param offset The offset from the beginning of the payload. * @return a p

* Update diagnostics with control loop timing information */

* External hook to trigger diagnostic update */

* Pull latest speed and travel measurements from MCU, and store in joint structure for ros_control */

* Get latest velocity commands from ros_control via joint structure, and send to MCU */

/////////////////////////////////////////////////////////////////////////// Subscribe on-demand

* Initialize Husky hardware */

/////////////////////////////////////////////////////////////////////////// Load the controller

* Constructs an unconfigured transport instance. */

* RobotHW provides velocity command in rad/s, Husky needs m/s, */

* Close this Transport. * @return Zero on success, nonzero otherwise * @post Tranport will be unconfigured, regardless of success/failure. */

* Closes the associated serial port */

* Control loop for Husky, not realtime safe */