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.

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_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_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.

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.

dir include/hddc2b/functions

dir include/hddc2b

dir include