/ * StateController initialiser function. Initialises member variables: robot state, gait selection and initalisation * flag and creates sub controll

* Returns a conversion of this pose object into a geometry_msgs::Pose convertToPoseMessage * @return The converted geometry_msgs::Pose message *

/ * Returns pointer to joint requested via identification name string input. * @param[in] joint_id_name The identification name of the requested join

* Converts Eigen Quaternion to Euler angles (intrinsic or extrinsic roll/pitch/yaw order) and enures values are in the * range -PI:PI. * @params[in]

* Transforms an input vector from the reference frame of this pose. * @params[in] vec The input vector to be transformed from this pose's reference

* Adds input pose to *this pose * @params[in] pose The pose to add from *this pose * @return The combination of *this pose and input pose */

/ * Applies inverse kinematics solution to achieve desired tip position. Clamps joint positions and velocities * within limits and applies forward ki

* Return rotation with shorter path between identical rotations on quaternion. * @params[in] test The test rotation to check for shorter rotation pat

/ * Sets desired tip pose to the input, applying admittance controller vertical offset (delta z) if requested. * @param[in] tip_pose The input desire

* Converts a linear control input (0.0 to 1.0) to a smoothed signal using the SmoothStep function. * (https://en.wikipedia.org/wiki/Smoothstep) * @p

* Converts Degrees to Radians. * @param[in] degrees Value in degrees to be converted to radians. */

* Checks for NaN values within pose elements * @return Bool denoting whether pose contains no NaN values */

/ * Uses bezier curves to smoothly update (over many iterations) the desired tip position of the leg associated with * this Leg Poser object, from th

/ * Returns pointer to leg requsted via identification name string input. * @param[in] leg_id_name The identification name of the requested leg objec

* Rounds the input number to the nearest integer. * @param[in] x The input number. */

* Generates a classical Denavit–Hartenberg (DH) matrix from DH parameters. * @param[in] d The DH parameter representing offset along previous z-axis

* Converts Euler Angles (intrisic or extrinsic roll/pitch/yaw order) to Eigen Quaternion * @param[in] euler Eigen Vector3d of Euler angles (roll, pit

* Calculates the projection vector of an input vector onto a reference vector. * (en.wikipedia.org/wiki/Vector_projection) * @param[in] a The input

* Returns the transformation matrix from the specified target joint of the robot model to this joint. * Target joint defaults to the origin of th

* Generates interpolation from this pose to target pose using control input between zero and one. * @params[in] control_input A value between 0.0 a

* Transforms this pose according to an input geometry_msgs::Transform * @params[in] transform The input transformation msg * @return The transfo

/ * Iterate through legs in robot model and directly move joints to positions defined by desired configuration. This * transition occurs simultaneous

/ * Generates child leg objects and copies state from reference model if provided. * Separated from constructor due to shared_from_this() constraints

/ * Generates universal workspace (map of radii for range of search bearings) for all legs by having each leg search * for joint limitations through

* Converts Radians to Degrees. * @param[in] radians Value in radians to be converted to degrees. */

* Rounds the input number to the nearest EVEN integer. * @param[in] x The input number. */

* Returns the input value with a precision defined by the precision input. (Eg: 1.00001 @ precision = 3 -> 1.000) * @param[in] value The input value.

/ * Updates joint default positions for each leg according to current joint positions of each leg. ***************************************************

/ * Scales virtual stiffness of an input swinging leg and two 'adjacent legs' according to an input reference. * The input reference ranges between 0

/ * Updates joint transforms and applies forward kinematics to calculate a new tip pose. * Sets leg current tip pose to new pose if requested. * @p

/ * Estimates the acceleration vector due to gravity from pitch and roll orientations from IMU data * @return The estimated acceleration vector due t

/ * Executes saved transition sequence in direction defined by 'sequence' (START_UP or SHUT_DOWN) through the use of the * function StepToPosition()

/ * Publishes visualisation markers which represent the robot model for display in RVIZ. Consists of line segments * linking the origin points of eac

* Calculates the rejection vector of an input vector onto a reference vector. * (en.wikipedia.org/wiki/Vector_projection) * @param[in] a The input v

Accessor for system state member */

/ * Initialises leg object by setting desired joint state to default values or to current position (from encoders) and * running forward kinematics f

/ * Iterate through legs in robot model and have them run their initialisation. * @param[in] use_default_joint_positions Flag denoting if the leg sho

Returns true if all joint objects in model have been initialised with a current position */

/ * Iterates through the legs in the robot model and generates a pose for each that is best for leg manipulation. This * pose is generated to attempt

* Removes input pose from *this pose * @params[in] pose The pose to remove from *this pose * @return The resultant pose after removing input pos

* Transforms an input vector into the reference frame of this pose. * @params[in] vec The input vector to be transformed into this pose's reference

/ * Updates the Step state of this LegStepper according to the phase *********************************************************************************

/ * Sets up velocities for and calls debug visualiser object to publish various debugging visualations via rviz **************************************

/ * Calculate maximum linear and angular speed/acceleration for each workspace radius in workspace map. These calculated * values will accomodate ove

/ * Calculates the change in pose over the desired time period assuming constant desired body velocity and walk plane. * @params[in] time_period The

/ * Iterates through legs in robot model and, in simulation, moves them in a linear trajectory directly from * their current tip position to its defa

/ * Estimates the acceleration vector due to gravity from pitch and roll orientations from IMU data * @return The estimated acceleration vector due t

/ * Publishes visualisation markers which represent the control nodes of the three bezier curves used to control tip * trajectory of the input leg.

/ * Generates a JointState message from the desired state of the joints of the leg object. * @param[out] joint_state_msg The output JointState mesag

/ * Publishes visualisation markers which represent the estimate of the gravitational acceleration vector. * @param[in] gravity_estimate An estimate

/ * Publishes visualisation markers which represent the estimated percentage of max torque in each joint. * @param[in] leg A pointer to the leg asso

/ * Publishes visualisation markers which represent requested stride vector for each leg. * @param[in] leg A pointer to the leg associated with the

/ * Publishes visualisation markers which represent an estimate of the terrain being traversed * @param[in] model A pointer to the robot model object

/ * Publishes visualisation markers which represent the calculated & measured estimated tip force vector for input leg. * @param[in] leg A pointer t

/ * Publishes visualisation markers which represent the trajectory of the tip of the input leg. * @param[in] leg A pointer to the leg associated with

/ * Publishes visualisation markers which represent the estimated walking plane. * @param[in] walk_plane A Vector representing the walk plane ******

* Initialisation function which self populates parameter data from ros parameter server. * @param[in] n Ros node handle to use to acquire data from

/ * Iterates through legs in robot model and generates and assigns a leg poser object. Calls function to initialise auto * pose objects. Seperated fr

/ * Initialises walk controller by setting desired default walking stance tip positions from parameters and creating * LegStepper objects for each le

Initialises the model by calling the model object function initLegs() */

/ * Iterates the step phase and updates the progress variables ***************************************************************************************

/ * Estimates if the robot is bearing its load on its legs. Estimates the average distance between body and leg tips * and checks if the average brea

/ * The main loop of the state controller (called from the main ros loop). * Coordinates with other controllers to update based on current robot stat

/ * Iterate through legs in robot model and directly move joints into 'packed' configuration as defined by joint * parameters. This maneuver occurs s

/ * Iterates through leg objects and either collates joint state information for combined publishing and/or publishes the * desired joint position on

/ * Publishes transforms linking odom, base_link and walk_plane frames. ******************************************************************************

/ * Iterates through leg objects and collates state information for publishing on custom leg state message topic * @todo Remove ASC state messages in

/ * Publishes current pose (roll, pitch, yaw, x, y, z) for debugging *********************************************************************************

/ * Publishes imu pose rotation absement, position and velocity errors used in the PID controller, for debugging *************************************

/ * Publishes details about current workspace (average/min/max radius) for debugging *****************************************************************

Resets all pose contributer variables to the identity pose */

/ * Initialises auto poser container and populates with auto poser class objects as defined by auto poser parameters. * Also sets auto pose parameter

/ * Calculates walk controller walk cycle parameters, normalising base parameters according to step frequency. ***************************************

Modifier for the time_delta_ member variable */

/ * Checks tip force magnitude against touchdown/liftoff thresholds to instantaneously define the location of the * step plane. *******************

/ * Iterate through legs in robot model and directly move tips to pose defined by target tip pose and target body pose. * This transition occurs sim

/ * Iterate through legs in robot model and directly move joints into 'unpacked' configuration as defined by joint * parameters. This maneuver occurs

/ * Iterates through legs in the robot model and updates the tip position offset value for each. * The calculation is achieved through the use of a c

/ * Depending on parameter flags, calls multiple posing functions and combines individual poses to update the current * desired pose of the robot mod

/ * Updates the tip position for legs in the manual state from tip velocity inputs. Two modes are available: joint * control allows manipulation of j

/ * Updates model configuration by applying inverse kinematics to solve desired tip poses generated from walk/pose * controllers. *******************

/ * Returns a pose which contributes to the auto pose applied to the robot body. The resultant pose is defined by a 4th * order bezier curve for both

/ * Iterates through legs in robot model and updates each Leg Poser tip position. This new tip position is the tip * position defined from the Leg St

/ * Updates position of tip using three quartic bezier curves to generate the tip trajectory. Calculates change in tip * position using two bezier cu

/ * Updates rotation of tip orthogonal to the plane of the body during swing period. Interpolation from origin rotation * to orthogonal rotation occu

/ * Updates all legs in the walk cycle. Calculates stride vectors for all legs from robot body velocity inputs and calls * trajectory update function

/ * AdmittanceController class constructor. Assigns pointers to robot model object and parameter data storage object. * @param[in] model Pointer to t

/ * Auto poser contructor. * @param[in] poser Pointer to the Pose Controller object * @param[in] id Int defining the id number of the created Auto P

/ * Constructor for debug visualiser class. Sets up publishers for the visualisation markers and initialises odometry. *******************************

* Returns pose with position and rotation elements set to identity values. * @return The Identity Pose */