API documentation

The API does not enforce any specific units of measurements, but all quantities must have consistent units. For instance, all lengths must be provided in metres.

By default, any array that contains measurements from or commands to a drive are arranged in right-before-left order. This convention aligns with the KELO Drive API where the right wheel has identifier \(1\) (index \(0\)) and the left wheel has identifier \(2\) (index \(1\)).

All matrices in hddc2b must be provided and will be returned in column-major storage order.

file drive.h

Functions

void hddc2b_drv_frc_gnd_to_pvt(int num_drv, const double *whl_dst, const double *cstr_off, const double *f_whl, double *f_drv)

Compute the force contribution of each of the two wheels to the drive’s attachment point.

Parameters:

• num_drv – [in] The number of drives that this function is applied to.

• whl_dst – [in] An array with num_drv elements that represent the (scalar) distance between each wheel and the central point between the wheels, i.e. the wheels are \(2 \cdot d_w\) apart from each other where \(w_d\) represents the wheel distance. The array is arranged as \( \begin{bmatrix} d_{w,1} & \ldots & d_{w,n} \end{bmatrix}\).

• cstr_off – [in] The castor offset, an array with num_drv elements that represent the (scalar) distance between the wheel axle and the drive’s attachment point (to the platform). The array is arranged as \( \begin{bmatrix} co_1 & \ldots & co_n \end{bmatrix}\).

• f_whl – [in] A matrix with two rows and num_drv columns where the rows contain the linear force at the right and left wheel-ground contact point, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,r} & \ldots & f_{n,r} \\ f_{1,l} & \ldots & f_{n,l} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• f_drv – [out] A matrix with two rows and num_drv columns where the rows represent the linear force components at the drive’s attachment point (to the platform) in the longitudinal and transverse direction, respectively. The forces’ coordinates are expressed in the individual drives’ pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,x} & \ldots & f_{n,x} \\ f_{1,y} & \ldots & f_{n,y} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

void hddc2b_drv_frc_pvt_to_gnd(int num_drv, const double *whl_dst, const double *cstr_off, const double *f_drv, double *f_whl)

Compute the force for each of the wheels in a wheel unit given the overall force at an attachment point.

Parameters:

• num_drv – [in] The number of drives that this function is applied to.

• whl_dst – [in] An array with num_drv elements that represent the (scalar) distance between each wheel and the central point between the wheels, i.e. the wheels are \(2 \cdot d_w\) apart from each other where \(w_d\) represents the wheel distance. The array is arranged as \( \begin{bmatrix} d_{w,1} & \ldots & d_{w,n} \end{bmatrix}\).

• cstr_off – [in] The castor offset, an array with num_drv elements that represent the (scalar) distance between the wheel axle and the drive’s attachment point (to the platform). The array is arranged as \( \begin{bmatrix} co_1 & \ldots & co_n \end{bmatrix}\).

• f_drv – [in] A matrix with two rows and num_drv columns where the rows represent the linear force components at the drive’s attachment point (to the platform) in the longitudinal and transverse direction, respectively. The forces’ coordinates are expressed in the individual drives’ pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,x} & \ldots & f_{n,x} \\ f_{1,y} & \ldots & f_{n,y} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• f_whl – [out] A matrix with two rows and num_drv columns where the rows contain the linear force at the right and left wheel-ground contact point, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,r} & \ldots & f_{n,r} \\ f_{1,l} & \ldots & f_{n,l} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

void hddc2b_drv_vel_gnd_to_pvt(int num_drv, const double *whl_dst, const double *cstr_off, const double *xd_whl, double *xd_drv)

Compute the velocity of the drive’s attachment point due to the two wheels’ velocities.

Parameters:

• num_drv – [in] The number of drives that this function is applied to.

• whl_dst – [in] An array with num_drv elements that represent the (scalar) distance between each wheel and the central point between the wheels, i.e. the wheels are \(2 \cdot d_w\) apart from each other where \(w_d\) represents the wheel distance. The array is arranged as \( \begin{bmatrix} d_{w,1} & \ldots & d_{w,n} \end{bmatrix}\).

• cstr_off – [in] The castor offset, an array with num_drv elements that represent the (scalar) distance between the wheel axle and the drive’s attachment point (to the platform). The array is arranged as \( \begin{bmatrix} co_1 & \ldots & co_n \end{bmatrix}\).

• xd_whl – [in] A matrix with two rows and num_drv columns where the rows contain the linear velocity at the right and left wheel-ground contact point, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,r} & \ldots & \dot{X}_{n,r} \\ \dot{X}_{1,l} & \ldots & \dot{X}_{n,l} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• xd_drv – [out] A matrix with two rows and num_drv columns where the rows represent the linear velocity components of the drive’s attachment point (to the platform) in the longitudinal and transverse direction, respectively. The linear velocities’ reference point is the origin of the respective pivot frames. Their coordinates are expressed in these pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,x} & \ldots & \dot{X}_{n,x} \\ \dot{X}_{1,y} & \ldots & \dot{X}_{n,y} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

void hddc2b_drv_vel_pvt_to_gnd(int num_drv, const double *whl_dst, const double *cstr_off, const double *xd_drv, double *xd_whl)

Compute the velocity at each of the two wheel-ground contact points given the velocity of the drive’s attachment point. This is the kinematic dual of the (force) mapping hddc2b_drv_frc_gnd_to_pvt (i.e. its transpose) and the inverse of hddc2b_drv_vel_gnd_to_pvt.

Parameters:

• num_drv – [in] The number of drives that this function is applied to.

• whl_dst – [in] An array with num_drv elements that represent the (scalar) distance between each wheel and the central point between the wheels, i.e. the wheels are \(2 \cdot d_w\) apart from each other where \(w_d\) represents the wheel distance. The array is arranged as \( \begin{bmatrix} d_{w,1} & \ldots & d_{w,n} \end{bmatrix}\).

• cstr_off – [in] The castor offset, an array with num_drv elements that represent the (scalar) distance between the wheel axle and the drive’s attachment point (to the platform). The array is arranged as \( \begin{bmatrix} co_1 & \ldots & co_n \end{bmatrix}\).

• xd_drv – [in] A matrix with two rows and num_drv columns where the rows represent the linear velocity components of the drive’s attachment point (to the platform) in the longitudinal and transverse direction, respectively. The linear velocities’ reference point is the origin of the respective pivot frames. Their coordinates are expressed in these pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,x} & \ldots & \dot{X}_{n,x} \\ \dot{X}_{1,y} & \ldots & \dot{X}_{n,y} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• xd_whl – [out] A matrix with two rows and num_drv columns where the rows contain the linear velocity at the right and left wheel-ground contact point, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,r} & \ldots & \dot{X}_{n,r} \\ \dot{X}_{1,l} & \ldots & \dot{X}_{n,l} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

void hddc2b_drv_vel_algn_dst(int num_drv, const double *xd_drv, double *dst, int inc_dst)

Compute a signed pivot alignment distance for each drive given its commanded attachment velocity. The distance is the angle between the drive’s rolling direction (the pivot frame’s x-axis) and the commanded velocity, i.e. \(dst_i = \operatorname{atan2}(\dot{X}_{i,y}, \dot{X}_{i,x})\). Rotating the pivot by this angle lets the drive roll without lateral scrubbing.

This is the velocity-domain counterpart of hddc2b_pltf_drv_algn_dst: because velocity distribution (hddc2b_pltf_vel_pltf_to_pvt) is a unique map, each drive’s desired rolling direction is determined exactly and can be read off directly, without the weighted heuristic and nullspace projection that the (redundant) force distribution requires.

Parameters:

• num_drv – [in] The number of drives that this function is applied to.

• xd_drv – [in] A matrix with two rows and num_drv columns where the rows represent the linear velocity components of the drive’s attachment point (to the platform) in the longitudinal and transverse direction, respectively. The coordinates are expressed in the individual drives’ pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,x} & \ldots & \dot{X}_{n,x} \\ \dot{X}_{1,y} & \ldots & \dot{X}_{n,y} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• dst – [out] An array with num_drv elements that represent the pivot alignment distance of each drive with respect to its commanded attachment velocity. The array is arranged as \( \begin{bmatrix} dst_1 & \ldots & dst_n \end{bmatrix}\).

• inc_dst – [in] Increment of the dst array.

void hddc2b_drv_vel_scrb_dst(int num_drv, const double *cstr_off, double omega_pltf, const double *xd_drv, double *dst, int inc_dst, double *spd, int inc_spd)

Compute the scrub angle of each drive under a platform twist. The scrub angle is the angle between the drive’s rolling direction (the pivot frame’s x-axis) and the velocity that the drive’s axle centre would have if the pivot did not rotate relative to the platform, i.e. if the castor turned with the platform at \(\omega_p\):

\[ dst_i = \operatorname{atan2}(\dot{X}_{i,y} - l_i \omega_p, \dot{X}_{i,x}) \]

where \(l_i\) is the castor offset. The scrub angle is zero exactly when the drive needs no pivot motion to follow the twist; that is the steady-state alignment, also while the platform rotates. In contrast to hddc2b_drv_vel_algn_dst the angle is defined with respect to the axle centre, not the pivot point.

A drive that rolls exactly backwards has a scrub angle of \(\pm\pi\) where the sign is numerically arbitrary. This function always returns \(+\pi\) so that all such drives are steered in the same, counter-clockwise, direction. For a vanishing axle velocity the scrub angle is zero.

Parameters:

• num_drv – [in] The number of drives that this function is applied to.

• cstr_off – [in] The castor offset, an array with num_drv elements that represent the (scalar) distance between the wheel axle and the drive’s attachment point (to the platform). The array is arranged as \( \begin{bmatrix} co_1 & \ldots & co_n \end{bmatrix}\).

• omega_pltf – [in] The platform’s angular velocity \(\omega_p\).

• xd_drv – [in] A matrix with two rows and num_drv columns where the rows represent the linear velocity components of the drive’s attachment point (to the platform) in the longitudinal and transverse direction, respectively. The coordinates are expressed in the individual drives’ pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,x} & \ldots & \dot{X}_{n,x} \\ \dot{X}_{1,y} & \ldots & \dot{X}_{n,y} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• dst – [out] An array with num_drv elements that represent the scrub angle of each drive. The array is arranged as \( \begin{bmatrix} dst_1 & \ldots & dst_n \end{bmatrix}\).

• inc_dst – [in] Increment of the dst array.

• spd – [out] An array with num_drv elements that represent the speed \(v_i\) of each drive’s axle centre, i.e. the magnitude of the velocity whose direction dst is. The array is arranged as \( \begin{bmatrix} v_1 & \ldots & v_n \end{bmatrix}\).

• inc_spd – [in] Increment of the spd array.

void hddc2b_drv_vel_algn_ref(int num_drv, const double *cstr_off, double tau, double qd_max, double omega_pltf, const double *dst, int inc_dst, const double *spd, int inc_spd, double *xd_ref, int inc_ref)

Compute the transverse pivot velocity that each drive requires to remove its scrub angle. A castor rotates at \(\dot{q}_i = \dot{X}_{i,y} / l_i\) in the world when its pivot point moves transversely at \(\dot{X}_{i,y}\), so the reference

\[ \dot{\bar{X}}_{i,y} = l_i \left( \operatorname{clip}\left( \min\left(\frac{1}{\tau}, \frac{v_i}{l_i}\right) dst_i, \pm\dot{q}_{max}\right) + \omega_p \right) \]

turns the castor with the platform and additionally drives the scrub angle to zero, at most at the rate \(\dot{q}_{max}\) relative to the platform.

A castor aligns by rolling: left to itself a trailing castor decays its scrub angle with the time constant \(l_i / v_i\), so at the speed \(v_i\) the drive currently rolls at that is as fast as the wheels can steer it without fighting the platform motion. The effective time constant is therefore the slower of \(\tau\) and \(l_i / v_i\), which also means that a drive at a standstill is not asked to swing at all.

Parameters:

• num_drv – [in] The number of drives that this function is applied to.

• cstr_off – [in] The castor offset, an array with num_drv elements that represent the (scalar) distance between the wheel axle and the drive’s attachment point (to the platform). The array is arranged as \( \begin{bmatrix} co_1 & \ldots & co_n \end{bmatrix}\).

• tau – [in] The shortest time constant \(\tau\) within which to remove the scrub angle. The time constant must be positive.

• qd_max – [in] The maximum castor rate \(\dot{q}_{max}\) with respect to the platform. The rate must be non-negative.

• omega_pltf – [in] The platform’s angular velocity \(\omega_p\).

• dst – [in] An array with num_drv elements that represent the scrub angle of each drive, as computed by hddc2b_drv_vel_scrb_dst. The array is arranged as \( \begin{bmatrix} dst_1 & \ldots & dst_n \end{bmatrix}\).

• inc_dst – [in] Increment of the dst array.

• spd – [in] An array with num_drv elements that represent the speed \(v_i\) of each drive’s axle centre, as computed by hddc2b_drv_vel_scrb_dst. The array is arranged as \( \begin{bmatrix} v_1 & \ldots & v_n \end{bmatrix}\).

• inc_spd – [in] Increment of the spd array.

• xd_ref – [out] An array with num_drv elements that represent the transverse velocity reference \(\bar{\dot{X}}_{i,y}\) of each drive’s attachment point. The coordinates are expressed in the individual drives’ pivot frames. The array is arranged as \( \begin{bmatrix} \bar{\dot{X}}_{1,y} & \ldots & \bar{\dot{X}}_{n,y} \end{bmatrix}\).

• inc_ref – [in] Increment of the xd_ref array. An increment of two writes the transverse row of a drive velocity matrix.

file platform.h

Functions

void hddc2b_pltf_drv_algn_dst(int num_drv, const double *pos_drv, const double *w, const double *q_pvt, const double *f_pltf, double *dst, int inc_dst)

Compute a signed and weighted distance between each drive’s orientation with respect to the platform-level task. This distance can be interpreted as a pivot torque that aligns a drive with the platform-level task. It is modeled as two competing tasks: one for the alignment with the angular part (the torque) of the task and the other for the alginment with the linear part (the force) of the task. The final command is the weighted sum of these two tasks.

Each drive has a particular orientation with respect to the platform - its actual configuration. For both tasks there exists a desired configuration:

• For the alignment with the linear part of the task, we simply interpret that linear part as a direction vector (complemented to a right-handed frame).

• For the alignment with the angular part of the task, we assume a best-case solution to produce a torque about the platform’s origin. Here, the drive should be orthogonal to the vector from the platform’s origin to the attachment’s origin. In other words, the “pushing” vector should be tangential to a circle around the platform’s origin with a radius equal to the length of the platform-attachment vector.

For each of the two desired configurations we can determine the orientation (rather an angular distance or “difference”) with respect to the actual configuration. This difference is to be interpreted as an angular velocity (to be executed over one unit of time). A “damping” or “weight” maps this angular velocity into a torque. The sum of the torque due to the angular and the linear task is added and returned by this function.

Better solution: don’t let the two tasks compete and coordinate implicitly (via a weighted sum) but reify the two tasks (each in their custom activity) and let them cooperate via explicit coordination (in yet another activity)!

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• pos_drv – [in] An array of num_drv position vectors that represent the position of each drive’s attachment point with respect to the platform’s origin. The coordinates are expressed in the platform frame. The array is arranged as

  \[\begin{split} \begin{bmatrix} x_1 & \ldots & x_n \\ y_1 & \ldots & y_n \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• w – [in] An array that represents a weight for the angular and the linear alignment tasks for each drive. The array is arranged as

  \[\begin{split} \begin{bmatrix} w_{ang,1} & \ldots & w_{ang,n} \\ w_{lin,1} & \ldots & w_{lin,n} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• q_pvt – [in] The array \(\vect{q}_{pvt}\) of num_drv pivot angles arranged as \( \begin{bmatrix} q_1 & \ldots & q_n \end{bmatrix}\).

• f_pltf – [in] The vector \(\vect{F}_p\) with three elements that represent a force and torque on the platform. The torque’s reference point is the platform’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} f_{p,x} & f_{p,y} & m_p \end{bmatrix}\).

• dst – [out] An array with num_drv elements that represent the pivot distances of each drive with respect to the platform-level task. The array is arranged as \( \begin{bmatrix} dst_1 & \ldots & dst_n \end{bmatrix}\).

• inc_dst – [in] Increment of the dst array.

void hddc2b_pltf_drv_vel_algn_dst(int num_drv, const double *pos_drv, const double *w, const double *q_pvt, const double *xd_pltf, double *dst, int inc_dst)

Compute a signed and weighted velocity-level pivot alignment reference for each drive with respect to a platform-level velocity task. This is the velocity-domain counterpart of hddc2b_pltf_drv_algn_dst: the returned distances can be used as transverse drive velocity references and projected into the nullspace of the primary platform-velocity task.

The angular task aligns each drive tangentially around the platform origin. The linear task aligns each drive with the commanded platform linear velocity. The final reference is the weighted sum of both signed alignment distances, scaled by the commanded angular and linear velocity, respectively.

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• pos_drv – [in] An array of num_drv position vectors that represent the position of each drive’s attachment point with respect to the platform’s origin. The coordinates are expressed in the platform frame. The array is arranged as

  \[\begin{split} \begin{bmatrix} x_1 & \ldots & x_n \\ y_1 & \ldots & y_n \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• w – [in] An array that represents a weight for the angular and the linear alignment tasks for each drive. The array is arranged as

  \[\begin{split} \begin{bmatrix} w_{ang,1} & \ldots & w_{ang,n} \\ w_{lin,1} & \ldots & w_{lin,n} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• q_pvt – [in] The array \(\vect{q}_{pvt}\) of num_drv pivot angles.

• xd_pltf – [in] The vector \(\dot{\vect{X}}_p\) with three elements that represent the platform linear and angular velocity.

• dst – [out] An array with num_drv elements that represent the velocity-level pivot alignment reference of each drive. The array is arranged as \( \begin{bmatrix} dst_1 & \ldots & dst_n \end{bmatrix}\).

• inc_dst – [in] Increment of the dst array.

void hddc2b_pltf_frc_comp_mat(int num_drv, const double *pos_drv, const double *q_pvt, double *g)

The force composition matrix \(\vect{G}(\vect{q}_{pvt}, \vec{\vect{p}}_d)\) depends on the pivot angles \(\vect{q}_{pvt}\) as well as the drive attachment vectors \(\vec{\vect{p}}_d\) and encodes (i) the change of the moment’s action point; and (ii) the change of coordinates, in both cases from the individual drives’ pivot frames to the platform frame.

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• pos_drv – [in] An array of num_drv position vectors \(\vec{\vect{p}}_d\) that represent the position of each drive’s attachment point with respect to the platform’s origin. The coordinates are expressed in the platform frame. The array is arranged as

  \[\begin{split} \begin{bmatrix} x_1 & \ldots & x_n \\ y_1 & \ldots & y_n \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• q_pvt – [in] The array \(\vect{q}_{pvt}\) of num_drv pivot angles arranged as \( \begin{bmatrix} q_1 & \ldots & q_n \end{bmatrix}\).

• g – [out] The force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns, measured in the platform’s origin and its coordinates expressed in the platform’s frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \frac{\partial{f_{p,x}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,x}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{n,y}}} \\ \frac{\partial{f_{p,y}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,y}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{n,y}}} \\ \frac{\partial{m_{p,z}}}{\partial{f_{1,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{m_{p,z}}}{\partial{f_{n,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{n,y}}} \end{bmatrix} \end{split}\]
  and will be provided in column-major order. Here, \(f_{p,x}\), \(f_{p,y}\) and \(m_{p,z}\) are the platform forces and torque, respectively. \(f_{i,x}\) and \(f_{i,y}\) are the drive’s forces.

void hddc2b_pltf_frc_pvt_to_pltf(int num_drv, const double *g, const double *f_drv, double *f_pltf)

Compute the platform-level force given drive-level forces. Since HDDC platforms are parallel kinematic chains, this mapping is unique. It is solved via the following formula:

\[ \vect{F}_p = \vect{G} \vect{F}_d \]

The input drive forces add to produce the desired platform force.

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• g – [in] The force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns, measured in the platform’s origin and its coordinates expressed in the platform’s frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \frac{\partial{f_{p,x}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,x}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{n,y}}} \\ \frac{\partial{f_{p,y}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,y}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{n,y}}} \\ \frac{\partial{m_{p,z}}}{\partial{f_{1,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{m_{p,z}}}{\partial{f_{n,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{n,y}}} \end{bmatrix} \end{split}\]
  and must be provided in column-major order. Here, \(f_{p,x}\), \(f_{p,y}\) and \(m_{p,z}\) are the platform forces and torque, respectively. \(f_{i,x}\) and \(f_{i,y}\) are the drive’s forces.

• f_drv – [in] The matrix \(\vect{F}_d\) with two rows and num_drv columns where the rows represent the linear force components that each drive exerts on the platform in the longitudinal and transverse direction, respectively. The forces’ coordinates are expressed in the individual drives’ pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,x} & \ldots & f_{n,x} \\ f_{1,y} & \ldots & f_{n,y} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• f_pltf – [out] The vector \(\vect{F}_p\) with three elements that represent force and torque applied to the platform due to the drives’ force contributions. The torque’s reference point is the platform’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} f_{p,x} & f_{p,y} & m_p \end{bmatrix}\).

void hddc2b_pltf_frc_w_pltf_sqrt(const double *w_pltf, double *w_pltf_sqrt)

\[\begin{split} \vect{Z}_p, \vect{\Lambda}_p &= \operatorname{dsyev}(\vect{W}_p) \\ \vect{W}_p^{\frac{1}{2}} &= \vect{Z}_p \vect{\Lambda}_p^{\frac{1}{2}} \vect{Z}_p^T \end{split}\]

Parameters:

• w_pltf – [in] The platform’s weight matrix \(\vect{W}_p\), a positive-definite \(3 \times 3\) matrix. The weight is measured in the platform’s origin and its coordinates are expressed in the platform frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} w_{p,xx} & w_{p,yx} & w_{p,mx} \\ w_{p,xy} & w_{p,yy} & w_{p,my} \\ w_{p,xm} & w_{p,ym} & w_{p,mm} \end{bmatrix} \end{split}\]
  where \(w_{p,yx} = w_{p,xy}\), \(w_{p,mx} = w_{p,xm}\) and \(w_{p,my} = w_{p,ym}\).

• w_pltf_sqrt – [out] The square root of the platform’s weight matrix \(\vect{W}_p^{\frac{1}{2}}\), a positive-definite \(3 \times 3\) matrix. The weight is measured in the platform’s origin and its coordinates are expressed in the platform frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} w_{p,xx}' & w_{p,yx}' & w_{p,mx}' \\ w_{p,xy}' & w_{p,yy}' & w_{p,my}' \\ w_{p,xm}' & w_{p,ym}' & w_{p,mm}' \end{bmatrix} \end{split}\]
  where \(w_{p,yx}' = w_{p,xy}'\), \(w_{p,mx}' = w_{p,xm}'\) and \(w_{p,my}' = w_{p,ym}'\).

void hddc2b_pltf_frc_w_drv_inv_sqrt(int num_drv, const double *w_drv, double *w_drv_inv_sqrt)

\[\begin{split} \vect{Z}_d, \vect{\Lambda}_d &= \operatorname{dsyev}(\vect{W}_d) \\ \vect{W}_d^{-\frac{1}{2}} &= \vect{Z}_d \vect{\Lambda}_d^{-\frac{1}{2}} \vect{Z}_d^T \end{split}\]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• w_drv – [in] An array that consists of num_drv positive-definite \(2 \times 2\) weight matrices \(\vect{W}_d\), each measured in the respective drive’s attachment point and its coordinates expressed in the drive’s attachment frame. The array is arranged as

  \[\begin{split} \begin{bmatrix} w_{1,xx} & \ldots & w_{n,xx} \\ w_{1,yx} & \ldots & w_{n,yx} \\ w_{1,xy} & \ldots & w_{n,xy} \\ w_{1,yy} & \ldots & w_{n,yy} \end{bmatrix} \end{split}\]
  where \(w_{i,yx} = w_{i,xy}\) and must be provided in column-major order.

• w_drv_inv_sqrt – [out] An array that consists of num_drv positive-definite \(2 \times 2\) weight matrices’ square roots \(\vect{W}_d^{-\frac{1}{2}}\), each measured in the respective drive’s attachment point and its coordinates expressed in the drive’s attachment frame. The array is arranged as

  \[\begin{split} \begin{bmatrix} w_{1,xx}' & \ldots & w_{n,xx}' \\ w_{1,yx}' & \ldots & w_{n,yx}' \\ w_{1,xy}' & \ldots & w_{n,xy}' \\ w_{1,yy}' & \ldots & w_{n,yy}' \end{bmatrix} \end{split}\]
  where \(w_{i,yx}' = w_{i,xy}'\) and will be provided in column-major order.

void hddc2b_pltf_frc_sing_wgh(int num_drv, const double *g_in, const double *f_pltf_in, const double *w_pltf_sqrt, double *g_out, double *f_pltf_out)

\[\begin{split} \vect{G}' &= \vect{W}_p^{\frac{1}{2}} \vect{G} \\ \vect{F}_p' &= \vect{W}_p^{\frac{1}{2}} \vect{F}_p \end{split}\]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• g_in – [in] The force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns, measured in the platform’s origin and its coordinates expressed in the platform’s frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \frac{\partial{f_{p,x}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,x}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{n,y}}} \\ \frac{\partial{f_{p,y}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,y}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{n,y}}} \\ \frac{\partial{m_{p,z}}}{\partial{f_{1,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{m_{p,z}}}{\partial{f_{n,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{n,y}}} \end{bmatrix} \end{split}\]
  and must be provided in column-major order. Here, \(f_{p,x}\), \(f_{p,y}\) and \(m_{p,z}\) are the platform forces and torque, respectively. \(f_{i,x}\) and \(f_{i,y}\) are the drive’s forces.

• f_pltf_in – [in] The vector \(\vect{F}_p\) with three elements that represent a force and torque on the platform. The torque’s reference point is the platform’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} f_{p,x} & f_{p,y} & m_p \end{bmatrix}\).

• w_pltf_sqrt – [in] The square root of the platform’s weight matrix \(\vect{W}_p^{\frac{1}{2}}\), a positive-definite \(3 \times 3\) matrix. The weight is measured in the platform’s origin and its coordinates are expressed in the platform frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} w_{p,xx}' & w_{p,yx}' & w_{p,mx}' \\ w_{p,xy}' & w_{p,yy}' & w_{p,my}' \\ w_{p,xm}' & w_{p,ym}' & w_{p,mm}' \end{bmatrix} \end{split}\]
  where \(w_{p,yx}' = w_{p,xy}'\), \(w_{p,mx}' = w_{p,xm}'\) and \(w_{p,my}' = w_{p,ym}'\).

• g_out – [out] The weighted force composition matrix \(\vect{G}'\) with three rows and \(2 \times {}\) num_drv columns. The matrix will be provided in column-major order.

• f_pltf_out – [out] The weighted force vector \(\vect{F}_p'\) with three elements.

void hddc2b_pltf_frc_redu_ref_init(int num_drv, const double *g, const double *f_pltf_in, const double *f_drv_ref, double *f_pltf_out)

\[ \vect{F}_p' = \vect{F}_p - \vect{G} \bar{\vect{F}}_d \]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• g – [in] (A version of) the force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns. The matrix must be provided in column-major order.

• f_pltf_in – [in] The vector \(\vect{F}_p\) with three elements that represent a force and torque on the platform. The torque’s reference point is the platform’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} f_{p,x} & f_{p,y} & m_p \end{bmatrix}\).

• f_drv_ref – [in] The matrix \(\bar{\vect{F}}_d\) with two rows and num_drv columns where the rows represent the linear force reference components in the longitudinal and transverse direction, respectively, that are projected into the nullspace of the platform task. The forces’ coordinates are expressed in the individual drives’ pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \bar{f}_{1,x} & \ldots & \bar{f}_{n,x} \\ \bar{f}_{1,y} & \ldots & \bar{f}_{n,y} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

• f_pltf_out – [out] The force vector \(\vect{F}_p'\) with three elements.

void hddc2b_pltf_frc_redu_wgh_init(int num_drv, const double *g_in, const double *w_drv_inv_sqrt, double *g_out)

\[ \vect{G}' = \vect{G} \vect{W}_d^{-\frac{1}{2}} \]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• g_in – [in] (A version of) the force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns. The matrix must be provided in column-major order.

• w_drv_inv_sqrt – [in] An array that consists of num_drv positive-definite \(2 \times 2\) weight matrices’ square roots \(\vect{W}_d^{-\frac{1}{2}}\), each measured in the respective drive’s attachment point and its coordinates expressed in the drive’s attachment frame. The array is arranged as

  \[\begin{split} \begin{bmatrix} w_{1,xx}' & \ldots & w_{n,xx}' \\ w_{1,yx}' & \ldots & w_{n,yx}' \\ w_{1,xy}' & \ldots & w_{n,xy}' \\ w_{1,yy}' & \ldots & w_{n,yy}' \end{bmatrix} \end{split}\]
  where \(w_{i,yx}' = w_{i,xy}'\) and must be provided in column-major order.

• g_out – [out] The weighted force composition matrix \(\vect{G}'\) with three rows and \(2 \times {}\) num_drv columns. The matrix will be provided in column-major order.

void hddc2b_pltf_frc_redu_wgh_fini(int num_drv, const double *f_drv_in, const double *w_drv_inv_sqrt, double *f_drv_out)

\[ \vect{F}_d = \vect{W}_d^{-\frac{1}{2}} \vect{F}_d' \]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• f_drv_in – [in] The matrix \(\vect{F}_d'\) with two rows and num_drv columns where the rows represent the linear force components that each drive exerts on the platform in the longitudinal and transverse direction, respectively. The forces’ coordinates are expressed in the individual drives’ pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,x}' & \ldots & f_{n,x}' \\ f_{1,y}' & \ldots & f_{n,y}' \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• w_drv_inv_sqrt – [in] An array that consists of num_drv positive-definite \(2 \times 2\) weight matrices’ inverse square roots \(\vect{W}_d^{-\frac{1}{2}}\), each measured in the respective drive’s attachment point and its coordinates expressed in the drive’s attachment frame. The array is arranged as

  \[\begin{split} \begin{bmatrix} w_{1,xx}' & \ldots & w_{n,xx}' \\ w_{1,yx}' & \ldots & w_{n,yx}' \\ w_{1,xy}' & \ldots & w_{n,xy}' \\ w_{1,yy}' & \ldots & w_{n,yy}' \end{bmatrix} \end{split}\]
  where \(w_{i,yx}' = w_{i,xy}'\) and must be provided in column-major order.

• f_drv_out – [out] The weighted matrix \(\vect{F}_d\) with two rows and num_drv columns. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,x} & \ldots & f_{n,x} \\ f_{1,y} & \ldots & f_{n,y} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

void hddc2b_pltf_frc_redu_ref_fini(int num_drv, const double *f_drv_in1, const double *f_drv_in2, double *f_drv_out)

\[ \vect{F}_d = \bar{\vect{F}}_d + \vect{F}_d' \]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• f_drv_in1 – [in] The matrix \(\bar{\vect{F}}_d\) with two rows and num_drv columns. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \bar{f}_{1,x} & \ldots & \bar{f}_{n,x} \\ \bar{f}_{1,y} & \ldots & \bar{f}_{n,y} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• f_drv_in2 – [in] The matrix \(\vect{F}_d'\) with two rows and num_drv columns. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,x}' & \ldots & f_{n,x}' \\ f_{1,y}' & \ldots & f_{n,y}' \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• f_drv_out – [out] The matrix \(\vect{F}_d\) with two rows and num_drv columns that represents the sum of both inputs. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,x} & \ldots & f_{n,x} \\ f_{1,y} & \ldots & f_{n,y} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

void hddc2b_pltf_frc_slv(int num_drv, const double *u, const double *s_inv, const double *vt, const double *f_pltf, double *f_drv)

\[ \vect{F}_d = \vect{V} \vect{S}^{-1} \vect{U}^T \vect{F}_p \]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• u – [in] The SVD’s \(\vect{U}\) matrix with three rows and three columns.

• s_inv – [in] A vector with some inverse of the three eigenvalues.

• vt – [in] The SVD’s \(\vect{V}^T\) matrix with \(2 \times {}\) num_drv rows and three columns.

• f_pltf – [in] The vector \(\vect{F}_p\) with three elements that represent a (version of a) force and torque on the platform. The torque’s reference point is the platform’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} f_{p,x} & f_{p,y} & m_p \end{bmatrix}\).

• f_drv – [out] The matrix \(\vect{F}_d\) with two rows and num_drv columns where the rows represent the linear force components that each drive exerts on the platform in the longitudinal and transverse direction, respectively. The forces’ coordinates are expressed in the individual drives’ pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,x} & \ldots & f_{n,x} \\ f_{1,y} & \ldots & f_{n,y} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

void hddc2b_pltf_vel_pltf_to_pvt(int num_drv, const double *g, const double *xd_pltf, double *xd_drv)

Compute the drive-level velocities given the platform-level velocity. Since HDDC platforms are parallel kinematic chains, this mapping is unique: each drive’s attachment velocity is fully determined by the (rigid) platform’s motion. It is the kinematic dual of the (force) composition \(\vect{F}_p = \vect{G} \vect{F}_d\) and is solved via the following formula:

\[ \dot{\vect{X}}_d = \vect{G}^T \dot{\vect{X}}_p \]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• g – [in] The force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns, measured in the platform’s origin and its coordinates expressed in the platform’s frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \frac{\partial{f_{p,x}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,x}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{n,y}}} \\ \frac{\partial{f_{p,y}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,y}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{n,y}}} \\ \frac{\partial{m_{p,z}}}{\partial{f_{1,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{m_{p,z}}}{\partial{f_{n,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{n,y}}} \end{bmatrix} \end{split}\]
  and must be provided in column-major order. Here, \(f_{p,x}\), \(f_{p,y}\) and \(m_{p,z}\) are the platform forces and torque, respectively. \(f_{i,x}\) and \(f_{i,y}\) are the drive’s forces.

• xd_pltf – [in] The vector \(\dot{\vect{X}}_p\) with three elements that represent the platform’s linear and angular velocity. The linear velocity’s reference point is the platform frame’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} \dot{X}_{p,x} & \dot{X}_{p,y} & \omega_p \end{bmatrix}\).

• xd_drv – [out] The matrix \(\dot{\vect{X}}_d\) with two rows and num_drv columns where the rows represent the linear velocity components of the drive’s attachment point (to the platform) in the longitudinal and transverse direction, respectively. The linear velocities’ reference point is the origin of the respective pivot frames. Their coordinates are expressed in these pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,x} & \ldots & \dot{X}_{n,x} \\ \dot{X}_{1,y} & \ldots & \dot{X}_{n,y} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

void hddc2b_pltf_vel_w_pltf_inv_sqrt(const double *w_pltf, double *w_pltf_inv_sqrt)

\[\begin{split} \vect{Z}_p, \vect{\Lambda}_p &= \operatorname{dsyev}(\vect{W}_p) \\ \vect{W}_p^{-\frac{1}{2}} &= \vect{Z}_p \vect{\Lambda}_p^{-\frac{1}{2}} \vect{Z}_p^T \end{split}\]

Parameters:

• w_pltf – [in] The platform’s weight matrix \(\vect{W}_p\), a positive-definite \(3 \times 3\) matrix. The weight is measured in the platform’s origin and its coordinates are expressed in the platform frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} w_{p,xx} & w_{p,yx} & w_{p,mx} \\ w_{p,xy} & w_{p,yy} & w_{p,my} \\ w_{p,xm} & w_{p,ym} & w_{p,mm} \end{bmatrix} \end{split}\]
  where \(w_{p,yx} = w_{p,xy}\), \(w_{p,mx} = w_{p,xm}\) and \(w_{p,my} = w_{p,ym}\).

• w_pltf_inv_sqrt – [out] The inverse square root of the platform’s weight matrix \(\vect{W}_p^{-\frac{1}{2}}\), a positive-definite \(3 \times 3\) matrix. The weight is measured in the platform’s origin and its coordinates are expressed in the platform frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} w_{p,xx}' & w_{p,yx}' & w_{p,mx}' \\ w_{p,xy}' & w_{p,yy}' & w_{p,my}' \\ w_{p,xm}' & w_{p,ym}' & w_{p,mm}' \end{bmatrix} \end{split}\]
  where \(w_{p,yx}' = w_{p,xy}'\), \(w_{p,mx}' = w_{p,xm}'\) and \(w_{p,my}' = w_{p,ym}'\).

void hddc2b_pltf_vel_w_drv_sqrt(int num_drv, const double *w_drv, double *w_drv_sqrt)

\[\begin{split} \vect{Z}_d, \vect{\Lambda}_d &= \operatorname{dsyev}(\vect{W}_d) \\ \vect{W}_d^{\frac{1}{2}} &= \vect{U}_d \vect{S}_d^{\frac{1}{2}} \vect{U}_d^T \end{split}\]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• w_drv – [in] An array that consists of num_drv positive-definite \(2 \times 2\) weight matrices \(\vect{W}_d\), each measured in the respective drive’s attachment point and its coordinates expressed in the drive’s attachment frame. The array is arranged as

  \[\begin{split} \begin{bmatrix} w_{1,xx} & \ldots & w_{n,xx} \\ w_{1,yx} & \ldots & w_{n,yx} \\ w_{1,xy} & \ldots & w_{n,xy} \\ w_{1,yy} & \ldots & w_{n,yy} \end{bmatrix} \end{split}\]
  where \(w_{i,yx} = w_{i,xy}\) and must be provided in column-major order.

• w_drv_sqrt – [out] An array that consists of num_drv positive-definite \(2 \times 2\) weight matrices’ square roots \(\vect{W}_d^{\frac{1}{2}}\), each measured in the respective drive’s attachment point and its coordinates expressed in the drive’s attachment frame. The array is arranged as

  \[\begin{split} \begin{bmatrix} w_{1,xx}' & \ldots & w_{n,xx}' \\ w_{1,yx}' & \ldots & w_{n,yx}' \\ w_{1,xy}' & \ldots & w_{n,xy}' \\ w_{1,yy}' & \ldots & w_{n,yy}' \end{bmatrix} \end{split}\]
  where \(w_{i,yx}' = w_{i,xy}'\) and will be provided in column-major order.

void hddc2b_pltf_vel_sing_wgh(int num_drv, const double *g_in, const double *xd_drv_in, const double *w_drv_sqrt, double *g_out, double *xd_drv_out)

\[\begin{split} \vect{G}' &= \vect{G} \vect{W}_d^{\frac{1}{2}}\\ \dot{\vect{X}}_d' &= \vect{W}_d^{\frac{1}{2}} \dot{\vect{X}}_d \end{split}\]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• g_in – [in] The force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns, measured in the platform’s origin and its coordinates expressed in the platform’s frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \frac{\partial{f_{p,x}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,x}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{n,y}}} \\ \frac{\partial{f_{p,y}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,y}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{n,y}}} \\ \frac{\partial{m_{p,z}}}{\partial{f_{1,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{m_{p,z}}}{\partial{f_{n,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{n,y}}} \end{bmatrix} \end{split}\]
  and must be provided in column-major order. Here, \(f_{p,x}\), \(f_{p,y}\) and \(m_{p,z}\) are the platform forces and torque, respectively. \(f_{i,x}\) and \(f_{i,y}\) are the drive’s forces.

• xd_drv_in – [in] The matrix \(\dot{\vect{X}}_d\) with two rows and num_drv columns where the rows represent the linear velocity components of the drive’s attachment point (to the platform) in the longitudinal and transverse direction, respectively. The linear velocities’ reference point is the origin of the respective pivot frames. Their coordinates are expressed in these pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,x} & \ldots & \dot{X}_{n,x} \\ \dot{X}_{1,y} & \ldots & \dot{X}_{n,y} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• w_drv_sqrt – [in] An array that consists of num_drv positive-definite \(2 \times 2\) weight matrices’ square roots \(\vect{W}_d^{\frac{1}{2}}\), each measured in the respective drive’s attachment point and its coordinates expressed in the drive’s attachment frame. The array is arranged as

  \[\begin{split} \begin{bmatrix} w_{1,xx}' & \ldots & w_{n,xx}' \\ w_{1,yx}' & \ldots & w_{n,yx}' \\ w_{1,xy}' & \ldots & w_{n,xy}' \\ w_{1,yy}' & \ldots & w_{n,yy}' \end{bmatrix} \end{split}\]
  where \(w_{i,yx}' = w_{i,xy}'\) and must be provided in column-major order.

• g_out – [out] The weighted force composition matrix \(\vect{G}'\) with three rows and \(2 \times {}\) num_drv columns. The matrix will be provided in column-major order.

• xd_drv_out – [out] The weighted drive velocity matrix \(\dot{\vect{X}}_d'\) with two rows and num_drv columns that will be provided in column-major order.

void hddc2b_pltf_vel_redu_ref_init(int num_drv, const double *g, const double *xd_drv_in, const double *xd_pltf_ref, double *xd_drv_out)

\[ \dot{\vect{X}}_d' = \dot{\vect{X}}_d - \vect{G}^T \bar{\dot{\vect{X}}}_p \]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• g – [in] (A version of) the force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns. The matrix must be provided in column-major order.

• xd_drv_in – [in] The matrix \(\dot{\vect{X}}_d\) with two rows and num_drv columns where the rows represent (a version of) the linear velocity components of the drive’s attachment point (to the platform) in the longitudinal and transverse direction, respectively. The linear velocities’ reference point is the origin of the respective pivot frames. Their coordinates are expressed in these pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,x} & \ldots & \dot{X}_{n,x} \\ \dot{X}_{1,y} & \ldots & \dot{X}_{n,y} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• xd_pltf_ref – [in] The vector \(\bar{\dot{\vect{X}}_p}\) with three elements that represent the platform’s linear and angular reference velocity that are projected into the nullspace of the drive task. The linear velocity’s reference point is the platform frame’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} \bar{\dot{X}}_{p,x} & \bar{\dot{X}}_{p,y} & \bar{\omega}_p \end{bmatrix}\).

• xd_drv_out – [out] The weighted drive velocity matrix \(\dot{\vect{X}}_d'\) with two rows and num_drv columns that will be provided in column-major order.

void hddc2b_pltf_vel_redu_wgh_init(int num_drv, const double *g_in, const double *w_pltf_inv_sqrt, double *g_out)

\[ \vect{G}' = \vect{W}_p^{-\frac{1}{2}} \vect{G} \]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• g_in – [in] (A version of) the force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns. The matrix must be provided in column-major order.

• w_pltf_inv_sqrt – [in] The inverse square root of the platform’s weight matrix \(\vect{W}_p^{-\frac{1}{2}}\), a positive-definite \(3 \times 3\) matrix. The weight is measured in the platform’s origin and its coordinates are expressed in the platform frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} w_{p,xx}' & w_{p,yx}' & w_{p,mx}' \\ w_{p,xy}' & w_{p,yy}' & w_{p,my}' \\ w_{p,xm}' & w_{p,ym}' & w_{p,mm}' \end{bmatrix} \end{split}\]
  where \(w_{p,yx}' = w_{p,xy}'\), \(w_{p,mx}' = w_{p,xm}'\) and \(w_{p,my}' = w_{p,ym}'\).

• g_out – [out] The weighted force composition matrix \(\vect{G}'\) with three rows and \(2 \times {}\) num_drv columns. The matrix will be provided in column-major order.

void hddc2b_pltf_vel_redu_wgh_fini(int num_drv, const double *xd_pltf_in, const double *w_pltf_inv_sqrt, double *xd_pltf_out)

\[ \dot{\vect{X}}_p = \vect{W}_p^{-\frac{1}{2}} \dot{\vect{X}}_p' \]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• xd_pltf_in – [out] The vector \(\dot{\vect{X}}_p\) with three elements that represent (a version of) the platform’s linear and angular velocity. The linear velocity’s reference point is the platform frame’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} \dot{X}_{p,x}' & \dot{X}_{p,y}' & \omega_p' \end{bmatrix}\).

• w_pltf_inv_sqrt – [in] The inverse square root of the platform’s weight matrix \(\vect{W}_p^{-\frac{1}{2}}\), a positive-definite \(3 \times 3\) matrix. The weight is measured in the platform’s origin and its coordinates are expressed in the platform frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} w_{p,xx}' & w_{p,yx}' & w_{p,mx}' \\ w_{p,xy}' & w_{p,yy}' & w_{p,my}' \\ w_{p,xm}' & w_{p,ym}' & w_{p,mm}' \end{bmatrix} \end{split}\]
  where \(w_{p,yx}' = w_{p,xy}'\), \(w_{p,mx}' = w_{p,xm}'\) and \(w_{p,my}' = w_{p,ym}'\).

• xd_pltf_out – [out] The vector \(\dot{\vect{X}}_p\) with three elements. The vector is arranged as \( \begin{bmatrix} \dot{X}_{p,x} & \dot{X}_{p,y} & \omega_p \end{bmatrix}\).

void hddc2b_pltf_vel_redu_ref_fini(const double *xd_pltf_in1, const double *xd_pltf_in2, double *xd_pltf_out)

\[ \dot{\vect{X}}_p = \bar{\dot{\vect{X}}}_p + \dot{\vect{X}}_p' \]

Parameters:

• xd_pltf_in1 – [in] The vector \(\bar{\dot{\vect{X}}}_p\) with three elements. The vector is arranged as \( \begin{bmatrix} \bar{\dot{X}}_x & \bar{\dot{X}}_y & \bar{\omega} \end{bmatrix}\).

• xd_pltf_in2 – [in] The vector \(\dot{\vect{X}}_p'\) with three elements. The vector is arranged as \( \begin{bmatrix} \dot{X}_x' & \dot{X}_y' & \omega' \end{bmatrix}\).

• xd_pltf_out – [out] The vector \(\dot{\vect{X}}_p\) with three elements. The vector is arranged as \( \begin{bmatrix} \dot{X}_x & \dot{X}_y & \omega \end{bmatrix}\).

void hddc2b_pltf_vel_slv(int num_drv, const double *u, const double *s_inv, const double *vt, const double *xd_drv, double *xd_pltf)

Note that the decomposition is of the \(\vect{G}\) matrix, but we need to work with the \(\vect{G}^T\) matrix. Hence, the SVD solving step may look “reversed”.

\[\begin{split} \vect{U}, \vect{S}, \vect{V}^T = \operatorname{desvg}(\vect{G}) &\Leftrightarrow \vect{V}, \vect{S}, \vect{U}^T = \operatorname{desvg}(\vect{G}^T) \\ &\Rightarrow \dot{\vect{X}}_p = \vect{U} \vect{S}^{-1} \vect{V}^T \dot{\vect{X}}_d \end{split}\]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• u – [in] The SVD’s \(\vect{U}\) matrix with three rows and three columns.

• s_inv – [in] A vector with some inverse of the three eigenvalues.

• vt – [in] The SVD’s \(\vect{V}^T\) matrix with \(2 \times {}\) num_drv rows and three columns.

• xd_drv – [in] The matrix \(\dot{\vect{X}}_d\) with two rows and num_drv columns where the rows represent the linear velocity components of the drive’s attachment point (to the platform) in the longitudinal and transverse direction, respectively. The linear velocities’ reference point is the origin of the respective pivot frames. Their coordinates are expressed in these pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,x} & \ldots & \dot{X}_{n,x} \\ \dot{X}_{1,y} & \ldots & \dot{X}_{n,y} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

• xd_pltf – [out] The vector \(\dot{\vect{X}}_p\) with three elements that represent (a version of) the platform’s linear and angular velocity. The linear velocity’s reference point is the platform frame’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} \dot{X}_x & \dot{X}_y & \omega \end{bmatrix}\).

void hddc2b_pltf_dcmp(int num_drv, const double *g, double *u, double *s, double *vt)

Compute a singular value decomposition (SVD) of a force composition matrix \(\vect{G}\).

\[ \vect{U}, \vect{S}, \vect{V}^T = \operatorname{dgesvd}(\vect{G}) \]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• g – [in] (A version of) the force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns. The matrix must be provided in column-major order.

• u – [out] The SVD’s \(\vect{U}\) matrix with three rows and three columns.

• s – [out] The SVD’s \(\vect{S}\) vector of eigenvalues with three elements. The vector entries will be sorted in descending order.

• vt – [out] The SVD’s \(\vect{V}^T\) matrix with \(2 \times {}\) num_drv rows and three columns.

void hddc2b_pltf_pinv(int num_drv, double eps, const double *s, double *s_inv)

Compute a pseudoinverse for each (eigen)value in a vector. The pseudoinverse is defined as:

\[\begin{split} S_i^{-1} = \begin{cases} 0 & \text{if } S < \epsilon \\ \frac{1}{S_i} & \text{else} \end{cases} \end{split}\]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• eps – [in] The scalar \(\epsilon\) that determines when to compute the inverse.

• s – [in] A vector of three eigenvalues that are either zero or positive.

• s_inv – [out] The inverse of the three eigenvalues.

void hddc2b_pltf_dmp(int num_drv, double eps, double lambda, const double *s, double *s_inv)

Compute a damped inverse for a vector of eigenvalues. The damping factor \(\lambda\) is only enabled in the vicinity (as measured by \(\epsilon\)) of a singularity according to [Chiaverini1991] :

\[\begin{split} \lambda_s &= \begin{cases} 0 & \text{if } S_{min} \ge \epsilon \\ \lambda \sqrt{1 - \left(\frac{S_{min}}{\epsilon}\right)^2} & \text{else} \end{cases} \\ S_i^{-1} &= \frac{S_i}{S_i^2 + \lambda_s^2} \end{split}\]

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• eps – [in] The scalar \(\epsilon\) that determines when to activate the damping.

• lambda – [in] The scalar \(\lambda\) that determines the “amount” of damping.

• s – [in] A vector of three eigenvalues that are either zero or positive. The vector entries must be sorted in descending order.

• s_inv – [out] A vector with the inverse of the three eigenvalues.

void hddc2b_pltf_vel_algn_rlx(int num_drv, const double *g, const double *w_pltf, const double *w_algn, const double *xd_pltf, const double *xd_drv_ref, double *xd_pltf_rlx)

Relax a commanded platform twist so that the drives can align their castors. The map from the platform twist to the pivot velocities, \(\dot{\vect{X}}_d = \vect{G}^T \dot{\vect{X}}_p\), is unique: the pivot points are rigidly attached to the platform, so there is no nullspace in which a castor alignment task could act without affecting the platform motion. Alignment is therefore posed as a lower-weighted task that modifies the twist itself. With \(\vect{a}_i^T\) the row of \(\vect{G}^T\) that yields the transverse velocity of pivot \(i\) and \(\dot{\bar{X}}_{i,y}\) the transverse velocity reference (see hddc2b_drv_vel_algn_ref), the relaxed twist is \(\dot{\vect{X}}_{rlx} = \dot{\vect{X}}_p + \vect{\delta}\) with

\[ \vect{\delta} = \arg\min_{\vect{\delta}} \vect{\delta}^T \vect{W}_p \vect{\delta} + \sum_i w_{a,i} \left( \vect{a}_i^T (\dot{\vect{X}}_p + \vect{\delta}) - \dot{\bar{X}}_{i,y} \right)^2 \]

which is the \(3 \times 3\) system \((\vect{W}_p + \sum_i w_{a,i} \vect{a}_i \vect{a}_i^T) \vect{\delta} = \sum_i w_{a,i} \vect{a}_i (\dot{\bar{X}}_{i,y} - \vect{a}_i^T \dot{\vect{X}}_p)\). The ratio of \(\vect{W}_p\) to \(w_{a,i}\) determines how much platform velocity is given up for alignment. When all references are already met the twist is returned unchanged.

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• g – [in] The force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns, measured in the platform’s origin and its coordinates expressed in the platform’s frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \frac{\partial{f_{p,x}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,x}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{n,y}}} \\ \frac{\partial{f_{p,y}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,y}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{n,y}}} \\ \frac{\partial{m_{p,z}}}{\partial{f_{1,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{m_{p,z}}}{\partial{f_{n,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{n,y}}} \end{bmatrix} \end{split}\]
  and must be provided in column-major order. Here, \(f_{p,x}\), \(f_{p,y}\) and \(m_{p,z}\) are the platform forces and torque, respectively. \(f_{i,x}\) and \(f_{i,y}\) are the drive’s forces.

• w_pltf – [in] The platform’s tracking weight matrix \(\vect{W}_p\), a positive-definite \(3 \times 3\) matrix. The weight is measured in the platform’s origin and its coordinates are expressed in the platform frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} w_{p,xx} & w_{p,yx} & w_{p,mx} \\ w_{p,xy} & w_{p,yy} & w_{p,my} \\ w_{p,xm} & w_{p,ym} & w_{p,mm} \end{bmatrix} \end{split}\]
  where \(w_{p,yx} = w_{p,xy}\), \(w_{p,mx} = w_{p,xm}\) and \(w_{p,my} = w_{p,ym}\) and must be provided in column-major order.

• w_algn – [in] An array with num_drv elements that represent a non-negative weight \(w_{a,i}\) for each drive’s castor alignment task. The array is arranged as \( \begin{bmatrix} w_{a,1} & \ldots & w_{a,n} \end{bmatrix}\).

• xd_pltf – [in] The vector \(\dot{\vect{X}}_p\) with three elements that represent the platform’s commanded linear and angular velocity. The linear velocity’s reference point is the platform frame’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} \dot{X}_{p,x} & \dot{X}_{p,y} & \omega_p \end{bmatrix}\).

• xd_drv_ref – [in] The matrix \(\dot{\bar{\vect{X}}}_d\) with two rows and num_drv columns where the second row represents the transverse velocity reference of each drive’s attachment point. The velocities’ coordinates are expressed in the individual drives’ pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{\bar{X}}_{1,x} & \ldots & \dot{\bar{X}}_{n,x} \\ \dot{\bar{X}}_{1,y} & \ldots & \dot{\bar{X}}_{n,y} \end{bmatrix} \end{split}\]
  and must be provided in column-major order. Only the second row is used.

• xd_pltf_rlx – [out] The vector \(\dot{\vect{X}}_{rlx}\) with three elements that represent the relaxed platform linear and angular velocity. The vector is arranged in the same way as xd_pltf.

void hddc2b_pltf_vel_hub_lim(int num_drv, double omega_max, const double *xd_pltf_in, const double *xd_drv_in, const double *omega_hub_in, double *xd_pltf_out, double *xd_drv_out, double *omega_hub_out)

Limit the wheel hub speeds by uniformly scaling a platform twist together with the pivot and hub velocities that were derived from it. If the largest absolute hub speed exceeds omega_max all three quantities are multiplied by \(\omega_{max} / \max_i |\omega_i|\), else they are passed through. As the maps from the platform twist to the pivot and hub velocities are linear the scaled quantities remain consistent with each other.

Each output may be the same array as the input it is derived from: the quantities are scaled element by element.

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• omega_max – [in] The maximum admissible hub speed \(\omega_{max}\). The speed must be positive.

• xd_pltf_in – [in] The vector \(\dot{\vect{X}}_p\) with three elements that represent the platform’s linear and angular velocity. The linear velocity’s reference point is the platform frame’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} \dot{X}_{p,x} & \dot{X}_{p,y} & \omega_p \end{bmatrix}\).

• xd_drv_in – [in] The matrix \(\dot{\vect{X}}_d\) with two rows and num_drv columns where the rows represent the linear velocity components of the drive’s attachment point (to the platform) in the longitudinal and transverse direction, respectively. The linear velocities’ reference point is the origin of the respective pivot frames. Their coordinates are expressed in these pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,x} & \ldots & \dot{X}_{n,x} \\ \dot{X}_{1,y} & \ldots & \dot{X}_{n,y} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• omega_hub_in – [in] A matrix with two rows and num_drv columns where the rows contain the angular velocity around the right and left wheel axle, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \omega_{1,r} & \ldots & \omega_{n,r} \\ \omega_{1,l} & \ldots & \omega_{n,l} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• xd_pltf_out – [out] The scaled platform twist, arranged in the same way as xd_pltf_in.

• xd_drv_out – [out] The scaled pivot velocities, arranged in the same way as xd_drv_in.

• omega_hub_out – [out] The scaled hub speeds, arranged in the same way as omega_hub_in.

file solver.h

Functions

void hddc2b_pltf_frc_pltf_to_drv(int num_drv, double eps, const double *g, const double *w_pltf, const double *f_pltf, const double *w_drv, const double *f_drv_ref, double *f_drv_prim, double *f_drv_scnd)

Find a solution to the force distribution problem for a situation where the platform is either singular (less than one drive unit) or redundant (two or more drive units), compute the weight decomposition internally and include a reference drive force in the nullspace:

\[\begin{split} \vect{F}_{prim} &= \vect{W}_d^{-\frac{1}{2}} \left(\vect{V} \vect{S}^+ \vect{U}^T\right) \vect{W}_p^{\frac{1}{2}} \vect{F}_p \\ \vect{F}_{scnd} &= \bar{\vect{F}}_d - \vect{W}_d^{-\frac{1}{2}} \left(\vect{V} \vect{S}^+ \vect{U}^T\right) \vect{W}_p^{\frac{1}{2}} \vect{G} \bar{\vect{F}}_d \end{split}\]

where \(\vect{U}, \vect{S}, \vect{V}^T\) is the singular value decomposition of \(\vect{W}_p^{\frac{1}{2}} \vect{G} \vect{W}_d^{-\frac{1}{2}}\) and \(\vect{S}^+\) its pseudoinverse.

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• eps – [in] The scalar \(\epsilon\) that determines when to compute the inverse.

• g – [in] The force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns, measured in the platform’s origin and its coordinates expressed in the platform’s frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \frac{\partial{f_{p,x}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,x}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{n,y}}} \\ \frac{\partial{f_{p,y}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,y}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{n,y}}} \\ \frac{\partial{m_{p,z}}}{\partial{f_{1,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{m_{p,z}}}{\partial{f_{n,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{n,y}}} \end{bmatrix} \end{split}\]
  and must be provided in column-major order. Here, \(f_{p,x}\), \(f_{p,y}\) and \(m_{p,z}\) are the platform forces and torque, respectively. \(f_{i,x}\) and \(f_{i,y}\) are the drive’s forces.

• w_pltf – [in] The platform’s weight matrix \(\vect{W}_p\), a positive-definite \(3 \times 3\) matrix. The weight is measured in the platform’s origin and its coordinates are expressed in the platform frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} w_{p,xx} & w_{p,yx} & w_{p,mx} \\ w_{p,xy} & w_{p,yy} & w_{p,my} \\ w_{p,xm} & w_{p,ym} & w_{p,mm} \end{bmatrix} \end{split}\]
  where \(w_{p,yx} = w_{p,xy}\), \(w_{p,mx} = w_{p,xm}\) and \(w_{p,my} = w_{p,ym}\).

• f_pltf – [in] The vector \(\vect{F}_p\) with three elements that represent a force and torque on the platform. The torque’s reference point is the platform’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} f_{p,x} & f_{p,y} & m_p \end{bmatrix}\).

• w_drv – [in] An array that consists of num_drv positive-definite \(2 \times 2\) weight matrices \(\vect{W}_d\), each measured in the respective drive’s attachment point and its coordinates expressed in the drive’s attachment frame. The array is arranged as

  \[\begin{split} \begin{bmatrix} w_{1,xx} & \ldots & w_{n,xx} \\ w_{1,yx} & \ldots & w_{n,yx} \\ w_{1,xy} & \ldots & w_{n,xy} \\ w_{1,yy} & \ldots & w_{n,yy} \end{bmatrix} \end{split}\]
  where \(w_{i,yx} = w_{i,xy}\) and must be provided in column-major order.

• f_drv_ref – [in] The matrix \(\bar{\vect{F}}_d\) with two rows and num_drv columns where the rows represent the linear force reference components in the longitudinal and transverse direction, respectively, that will be projected into the nullspace of the platform task. The forces’ coordinates are expressed in the individual drives’ pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \bar{f}_{1,x} & \ldots & \bar{f}_{n,x} \\ \bar{f}_{1,y} & \ldots & \bar{f}_{n,y} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• f_drv_prim – [out] The matrix \(\vect{F}_{prim}\) with two rows and num_drv columns that represents the primary-task drive forces resulting from distributing the platform-level force f_pltf. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,x} & \ldots & f_{n,x} \\ f_{1,y} & \ldots & f_{n,y} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

• f_drv_scnd – [out] The matrix \(\vect{F}_{scnd}\) with two rows and num_drv columns that represents the part of f_drv_ref projected into the nullspace of the platform-level task. The matrix is arranged the same as f_drv_prim and will be provided in column-major order. The combined drive force is \(\vect{F}_d = \vect{F}_{prim} + \vect{F}_{scnd}\).

void hddc2b_pltf_vel_pltf_to_drv(int num_drv, double eps, const double *g, const double *w_drv_sqrt, const double *xd_pltf, const double *w_pltf_inv_sqrt, const double *xd_drv_ref, double *xd_drv_prim, double *xd_drv_scnd)

The kinematic dual of hddc2b_pltf_frc_pltf_to_drv() for a situation where the platform is either singular (less than one drive unit) or redundant (two or more drive units): the primary task is the unique velocity distribution for the commanded platform twist, and the secondary task keeps only the part of a drive-space reference velocity that does not induce any platform twist under the same weighted velocity-composition model (with weights w_drv_sqrt and w_pltf_inv_sqrt) used to compose a platform velocity from wheel velocities:

\[\begin{split} \dot{\vect{X}}_{prim} &= \vect{G}^T \dot{\vect{X}}_p \\ \dot{\vect{X}}_{scnd} &= \dot{\vect{X}}_{ref} - \vect{G}^T \vect{W}_p^{-\frac{1}{2}} \left(\vect{U} \vect{S}^+ \vect{V}^T\right) \vect{W}_d^{\frac{1}{2}} \dot{\vect{X}}_{ref} \end{split}\]

where \(\vect{U}, \vect{S}, \vect{V}^T\) is the singular value decomposition of \(\vect{W}_p^{-\frac{1}{2}} \vect{G} \vect{W}_d^{\frac{1}{2}}\) and \(\vect{S}^+\) its pseudoinverse.

Parameters:

• num_drv – [in] The number of drives that the platform consists of.

• eps – [in] The scalar \(\epsilon\) that determines when to compute the inverse.

• g – [in] The force composition matrix \(\vect{G}\) with three rows and \(2 \times {}\) num_drv columns, measured in the platform’s origin and its coordinates expressed in the platform’s frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \frac{\partial{f_{p,x}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,x}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,x}}}{\partial{f_{n,y}}} \\ \frac{\partial{f_{p,y}}}{\partial{f_{1,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{f_{p,y}}}{\partial{f_{n,x}}} & \frac{\partial{f_{p,y}}}{\partial{f_{n,y}}} \\ \frac{\partial{m_{p,z}}}{\partial{f_{1,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{1,y}}} & \ldots & \frac{\partial{m_{p,z}}}{\partial{f_{n,x}}} & \frac{\partial{m_{p,z}}}{\partial{f_{n,y}}} \end{bmatrix} \end{split}\]
  and must be provided in column-major order. Here, \(f_{p,x}\), \(f_{p,y}\) and \(m_{p,z}\) are the platform forces and torque, respectively. \(f_{i,x}\) and \(f_{i,y}\) are the drive’s forces.

• w_drv_sqrt – [in] An array that consists of num_drv positive-definite \(2 \times 2\) weight matrices’ square roots \(\vect{W}_d^{\frac{1}{2}}\), each measured in the respective drive’s attachment point and its coordinates expressed in the drive’s attachment frame. The array is arranged as

  \[\begin{split} \begin{bmatrix} w_{1,xx}' & \ldots & w_{n,xx}' \\ w_{1,yx}' & \ldots & w_{n,yx}' \\ w_{1,xy}' & \ldots & w_{n,xy}' \\ w_{1,yy}' & \ldots & w_{n,yy}' \end{bmatrix} \end{split}\]
  where \(w_{i,yx}' = w_{i,xy}'\) and must be provided in column-major order.

• xd_pltf – [in] The vector \(\dot{\vect{X}}_p\) with three elements that represent the linear and angular velocity of the platform. The linear velocity’s reference point is the platform’s origin. The coordinates are expressed in the platform frame. The vector is arranged as \( \begin{bmatrix} v_{p,x} & v_{p,y} & \omega_p \end{bmatrix}\).

• w_pltf_inv_sqrt – [in] The inverse square root of the platform’s weight matrix \(\vect{W}_p^{-\frac{1}{2}}\), a positive-definite \(3 \times 3\) matrix. The weight is measured in the platform’s origin and its coordinates are expressed in the platform frame. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} w_{p,xx}' & w_{p,yx}' & w_{p,mx}' \\ w_{p,xy}' & w_{p,yy}' & w_{p,my}' \\ w_{p,xm}' & w_{p,ym}' & w_{p,mm}' \end{bmatrix} \end{split}\]
  where \(w_{p,yx}' = w_{p,xy}'\), \(w_{p,mx}' = w_{p,xm}'\) and \(w_{p,my}' = w_{p,ym}'\).

• xd_drv_ref – [in] The matrix \(\bar{\dot{\vect{X}}}_d\) with two rows and num_drv columns where the rows represent the linear velocity reference components in the longitudinal and transverse direction, respectively, of which the part that does not induce a platform twist will be returned in xd_drv_scnd. The velocities’ coordinates are expressed in the individual drives’ pivot frames. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \bar{\dot{X}}_{1,x} & \ldots & \bar{\dot{X}}_{n,x} \\ \bar{\dot{X}}_{1,y} & \ldots & \bar{\dot{X}}_{n,y} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• xd_drv_prim – [out] The matrix \(\dot{\vect{X}}_{prim}\) with two rows and num_drv columns that represents the primary-task drive velocities resulting from distributing the platform twist xd_pltf. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,x} & \ldots & \dot{X}_{n,x} \\ \dot{X}_{1,y} & \ldots & \dot{X}_{n,y} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

• xd_drv_scnd – [out] The matrix \(\dot{\vect{X}}_{scnd}\) with two rows and num_drv columns that represents the part of xd_drv_ref that does not induce a platform twist. The matrix is arranged the same as xd_drv_prim and will be provided in column-major order.

file wheel.h

Functions

void hddc2b_whl_frc_hub_to_gnd(int num_drv, const double *whl_dia, const double *tau_hub, double *f_whl)

Solve the wheels’ forward force kinematics problem, i.e. from actuator torque to (linear) force at the wheel-ground contact point.

Parameters:

• num_drv – [in] The number of drives that this function is applied to.

• whl_dia – [in] An array with two rows and num_drv columns where the rows contain the diameter of the right and left wheel, respectively. The array is arranged as

  \[\begin{split} \begin{bmatrix} d_{1,r} & \ldots & d_{n,r} \\ d_{1,l} & \ldots & d_{n,l} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• tau_hub – [in] A matrix with two rows and num_drv columns where the rows contain the actuator torque around the right and left wheel axle, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \tau_{1,r} & \ldots & \tau_{n,r} \\ \tau_{1,l} & \ldots & \tau_{n,l} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• f_whl – [out] A matrix with two rows and num_drv columns where the rows contain the linear force at the right and left wheel-ground contact point, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,r} & \ldots & f_{n,r} \\ f_{1,l} & \ldots & f_{n,l} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

void hddc2b_whl_frc_gnd_to_hub(int num_drv, const double *whl_dia, const double *f_whl, double *tau_hub)

Solve the wheels’ inverse force kinematics problem, i.e. from (linear) force at the wheel-ground contact point to actuator torque.

Parameters:

• num_drv – [in] The number of drives that this function is applied to.

• whl_dia – [in] An array with two rows and num_drv columns where the rows contain the diameter of the right and left wheel, respectively. The array is arranged as

  \[\begin{split} \begin{bmatrix} d_{1,r} & \ldots & d_{n,r} \\ d_{1,l} & \ldots & d_{n,l} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• f_whl – [in] A matrix with two rows and num_drv columns where the rows contain the linear force at the right and left wheel-ground contact point, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} f_{1,r} & \ldots & f_{n,r} \\ f_{1,l} & \ldots & f_{n,l} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• tau_hub – [out] A matrix with two rows and num_drv columns where the rows contain the actuator torque around the right and left wheel axle, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \tau_{1,r} & \ldots & \tau_{n,r} \\ \tau_{1,l} & \ldots & \tau_{n,l} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

void hddc2b_whl_vel_hub_to_gnd(int num_drv, const double *whl_dia, const double *omega_hub, double *xd_whl)

Solve the wheels’ forward velocity kinematics problem, i.e. from actuator velocity to linear velocity at the wheel-ground contact point.

Parameters:

• num_drv – [in] The number of drives that this function is applied to.

• whl_dia – [in] An array with two rows and num_drv columns where the rows contain the diameter of the right and left wheel, respectively. The array is arranged as

  \[\begin{split} \begin{bmatrix} d_{1,r} & \ldots & d_{n,r} \\ d_{1,l} & \ldots & d_{n,l} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• omega_hub – [in] A matrix with two rows and num_drv columns where the rows contain the angular velocity around the right and left wheel axle, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \omega_{1,r} & \ldots & \omega_{n,r} \\ \omega_{1,l} & \ldots & \omega_{n,l} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• xd_whl – [out] A matrix with two rows and num_drv columns where the rows contain the linear velocity at the right and left wheel-ground contact point, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,r} & \ldots & \dot{X}_{n,r} \\ \dot{X}_{1,l} & \ldots & \dot{X}_{n,l} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

void hddc2b_whl_vel_gnd_to_hub(int num_drv, const double *whl_dia, const double *xd_whl, double *omega_hub)

Solve the wheels’ inverse velocity kinematics problem, i.e. from linear velocity at the wheel-ground contact point to actuator velocity. This is the kinematic dual of hddc2b_whl_frc_gnd_to_hub and the inverse of hddc2b_whl_vel_hub_to_gnd.

Parameters:

• num_drv – [in] The number of drives that this function is applied to.

• whl_dia – [in] An array with two rows and num_drv columns where the rows contain the diameter of the right and left wheel, respectively. The array is arranged as

  \[\begin{split} \begin{bmatrix} d_{1,r} & \ldots & d_{n,r} \\ d_{1,l} & \ldots & d_{n,l} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• xd_whl – [in] A matrix with two rows and num_drv columns where the rows contain the linear velocity at the right and left wheel-ground contact point, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \dot{X}_{1,r} & \ldots & \dot{X}_{n,r} \\ \dot{X}_{1,l} & \ldots & \dot{X}_{n,l} \end{bmatrix} \end{split}\]
  and must be provided in column-major order.

• omega_hub – [out] A matrix with two rows and num_drv columns where the rows contain the angular velocity around the right and left wheel axle, respectively. The matrix is arranged as

  \[\begin{split} \begin{bmatrix} \omega_{1,r} & \ldots & \omega_{n,r} \\ \omega_{1,l} & \ldots & \omega_{n,l} \end{bmatrix} \end{split}\]
  and will be provided in column-major order.

dir include/hddc2b/functions

dir include/hddc2b

dir include