Comprehensive theory of differential kinematics and dynamics towards extensive motion optimization framework

Abstract

This paper presents a novel unified theoretical framework for differential kinematics and dynamics for the optimization of complex robot motion. By introducing an 18×18 comprehensive motion transformation matrix, the forward differential kinematics and dynamics, including velocity and acceleration, can be written in a simple chain product similar to an ordinary rotational matrix. This formulation enables the analytical computation of derivatives of various physical quantities (e.g. link velocities, link accelerations, or joint torques) with respect to joint coordinates, velocities and accelerations for a robot trajectory in an efficient manner ( $O\left(N_J\right)$ , where $N_J$ is the number of the robot’s degree of freedom), which is useful for motion optimization. Practical implementation of gradient computation is demonstrated together with simulation results of robot motion optimization to validate the effectiveness of the proposed framework.

1. Introduction

In recent robotics research, the importance of optimization has increased in a variety of aspects. Classic inverse kinematics and inverse dynamics problems (Nakamura, 1991) can be handled as optimization problems of joint coordinates or torques subject to some constraints that are derived from physical consistency or desired tasks. Some practical optimization frameworks have been proposed and applied to not only motion planning and control of a humanoid robot (Escande et al., 2014; Suleiman et al., 2008) but also to motion reconstruction (Yamane and Nakamura, 2003), contact estimation, and musculoskeletal analysis (Delp and Loan, 2000; Nakamura et al., 2005) of a digital human model. The model-identification problem (Khalil and Dombre, 2002) is also an optimization problem with respect to the model parameters with inverse dynamics computations. In each of these basic problems, only one type of physical quantity is optimized. For example, inverse kinematics computes the joint coordinates, inverse dynamics the joint torques, identification of the inertial parameters, etc.

However, practical problems are usually combinations of different problems, and different physical quantities must be simultaneously optimized for several different times. Trajectory optimization subject to physical consistent conditions is a typical example. In locomotion planning, because the violation of conditions at a certain time instance leads to future risk of falls, several sets of joint coordinates during a certain period often must be optimized simultaneously to predict future risks. Because this optimization usually requires a huge computational cost, the balancing problem is often simplified, perhaps by utilizing a low-dimensional model (Kajita et al., 2003a). Another example is the kinematic calibration of human body segments (Kirk et al., 2005) from motion capture measurements because the joint angles cannot be measured directly by encoders unlike in standard robot calibration (Khalil and Dombre, 2002). This application requires simultaneous optimization of the geometric parameters and the generalized coordinates (Ayusawa and Yoshida, 2017b).

More recently, the application of optimization has been intensively investigated in the field of anthropomorphic systems: humanoid robotics and human motion analysis. Humanoid robots are expected to execute more complicated and practical tasks such as disaster response (Lim et al., 2016), evaluation of human oriented products as a physical human simulator (Miura et al., 2013), etc. In such applications, anthropomorphic motion optimization faces far more complex problems combining modeling, kinematics, dynamics, planning, and control. For instance, the identification of the inertial parameters of a humanoid robot is important to realize precise and dynamic control (Ayusawa et al., 2014). However, the optimal motion generation to maximize the total identification performance is a complex problem of trajectory optimization and a balancing problem (Bonnet et al., 2016). In humanoid applications such as an active dummy for assistive device evaluation, imitation of human-like motion is necessary. This technique, called motion retargeting (Gleicher, 1998; Pollard et al., 2002), usually involves the inverse kinematics problem for both the human and humanoid, identification of the morphing function, and motion control of a robot while considering physical consistency. Yet another example is human simulators: recent detailed simulators are often connected to other simulation systems such as deformation computation (e.g. dynamics simulation using finite element method (FEM) (Allard et al., 2007).) Those simulators are often used to optimize the motion of a digital human or parameters of a product to be designed with the simulator. In such problems, we usually solve a simultaneous problem with modeling, kinematics, and dynamics problems (Ayusawa and Yoshida, 2017b). However, the above issues are currently difficult to solve, and some of them are still open problems due to the complexity of the derivative computation as described below.

To establish a comprehensive optimization framework that_ can handle critical issues required from the computational and practical points of view, the partial derivative of any physical quantities with respect to the joint coordinates is important when evaluating various types of conditions represented by the coordinates, derivatives, and forces in both Cartesian and joint space, even though some optimization techniques do not require the derivatives. Suleiman et al. (2008) developed the fundamental framework of humanoid motion optimization. Their work utilized the works of Park et al. (1995) and Sohl and Bobrow (2000) and formulated the analytical partial derivatives of the Cartesian coordinates, their derivatives, and joint torques with respect to the joint coordinates and their derivatives. Despite its possibility of extension to handle the many types of problems mentioned above, two issues remain. First, because the formulation mainly focuses on the manifold of Cartesian spaces, it is difficult to handle a free-floating base and spherical joints that are often used in human and humanoid kinematics modeling (Yamane, 2004). To represent the orientation of those joints, the generalized coordinates need to contain the rotation manifolds. The previous work has therefore difficulty in formulating the partial derivatives to handle the differential relationship of manifolds between the Cartesian and generalized coordinates. The second issue is the computational complexity; if we compute the partial derivative of the joint torque for the base-link, the computational complexity is proportional to the square of the degree of freedom (DOF), i.e. O ( $N_J^2$ ), where $N_J$ is the number of DOF. This leads to huge computational costs for the optimization when dealing with a large-DOF system. In the field of computer animation, Fang and Pollard (2003) presented an efficient method to compute the Jacobian of the total force acting on the whole body with computational complexity O ( $N_J$ ). Unfortunately, the formulation does not provide the Jacobian for the joint constraint force or joint torque, except for the root link. An efficient formulation about the Jacobian for any joints needs to be investigated.

In this paper, we reformulate the differential kinematics and dynamics for the fast computation of the analytical partial derivatives of Cartesian variables and generalized forces with respect to the joint coordinates and their derivatives. We introduce an 18-dimensional Comprehensive Motion Transformation Matrix (CMTM) in order to formulate the standard forward differential kinematics problem. This formulation makes it possible to reduce the computation of differential forward kinematics of kinematic chain to a simple chain product of the matrices in a similar manner to the standard rotational matrix, or the 6-dimensional matrix used in adjoint map on SE(3) (Park et al., 1995). The CMTM also allows the formulation of an analytical form of several partial derivatives with respect to the joint coordinates and theirderivatives including different types of joints. The partial derivatives of link variables are extended form of the basic Jacobian matrix (Khatib, 1987), and can be derived from the same formulation used in the basic Jacobian. The Jacobian of the joint torque is also extended from the linear/angular momentum Jacobian (Kajita et al., 2003b; Sugihara and Nakamura, 2002), which is also formulated in the same manner via the CMTM. The analytical derivative of physical quantities such as the zero moment point (ZMP) (Vukobratovic et al., 1970) can be easily computed with the proposed method. In addition, each computational cost of the new Jacobians is O( $N_J$ ). A recent computational technique called automatic differentiation is also expected to compute the Jacobian matrix with O( $N_J$ ). Though the automatic differentiation cannot provide the symbolic formula unlike algebraic differentiation, it can quickly compute the derivatives with high accuracy in contrast to numerical differentiation. The computational speed of the proposed method is also compared to the automatic differentiation.

Though it is important to derive the Jacobian matrices theoretically, from a practical point of view in the motion optimization, the direct computation of the Jacobian matrices is not always computationally efficient. When computing the gradient of the cost function or the function for each constraint, its computational cost can be further reduced by decomposing the gradient computation into the combination of kinematic and dynamics computation (Ayusawa and Nakamura, 2012). The decomposed gradient computation (DGC) does not require the direct computation of the Jacobian matrix. Based on our previous work (Ayusawa and Yoshida, 2017a), this paper newly presents the efficient decomposed gradient computation for the proposed comprehensive theory for motion optimization. Numerical simulations of computational cost and simulation results of motion optimization for a redundant manipulator and a humanoid robot are provided to demonstrate the effectiveness of the proposed method.

2. Motion optimization framework

This section presents the overview of the motion optimization problem and the flow of the computation. Let the generalized coordinates of a robot be $\mathit{q}$ , with their trajectories parameterized by $\mathit{a}$ and time instance t: $\mathit{q}\left(\mathit{a},t\right)$ . In this paper, the trajectories are represented by, for example, polynomial interpolation, Fourier series, B-splines, etc. Their derivatives $\stackrel{.}{\mathit{q}}$ and $\stackrel{··}{\mathit{q}}$ are computed with $\mathit{a}$ and t according to the implemented trajectory parameterization.

Let us concatenate $\mathit{q}$ , $\stackrel{.}{\mathit{q}}$ , and $\stackrel{··}{\mathit{q}}$ into $\mathit{x}$ , and consider the physical quantities $\mathit{y}$ that are represented by $\mathit{x}$ . The candidates of $\mathit{y}$ could be, for example, the position, orientation, linear and angular velocity, linear and angular acceleration of each link coordinate, the joint torques and the constraint forces acting on the joint coordinates, etc. Let ${\mathit{y}}_{i,j}$ denotes i th quantity at the j th time instance $t_j$ , ${\mathit{x}}_j$ the coordinates and their derivatives at $t_j$ , and $\mathit{Y}$ the entire set of the quantities to be evaluated. In this paper, the set of time instances $t_j$ is given and constant, and the following optimization problem is to be solved

$$\begin{array}{c}\underset{\mathit{a}}{min}c\left(\mathit{Y}\right)\hfill \\ \mathrm{subject\; to}\forall k{g}_k\left(\mathit{Y}\right)\le 0\hfill \end{array}$$

(1)

where, c is the cost function to be evaluated, and $g_k$ is k th inequality constraint. Because equality constraints can be represented by two inequality constraints, they are summarized and represented by the inequality form.

The above optimization problem is usually computationally expensive. To increase the computational speed, efficient optimization techniques often require analytical gradient computation of the cost function and each constraint. The gradient can be decomposed as follows

$$\begin{array}{c}\frac{\partial h}{\partial \mathit{a}}=\sum _i\sum _j\frac{\partial h}{\partial {\mathit{y}}_{i,j}}\frac{\partial {\mathit{y}}_{i,j}}{\partial {\mathit{x}}_j}\frac{\partial {\mathit{x}}_j}{\partial \mathit{a}}\left(h=c\mathrm{or}{g}_k\right)\hfill \end{array}$$

(2)

The gradient $\partial h∕\partial {\mathit{y}}_{i,j}$ is determined by the form of the cost function or the constraints. The partial derivative $\partial {\mathit{x}}_j∕\partial \mathit{a}$ can be computed from the implemented trajectory parameterization. The term $\partial {\mathit{y}}_{i,j}∕\partial {\mathit{x}}_j$ represents the partial derivative of several types of quantities of multi-body systems with respect to the joint coordinates and their derivatives. A typical example is the partial derivative of the position and orientation of each link with respect to the joint coordinates, which is known as the basic Jacobian (Khatib, 1987). In this case, the derivatives of the velocities, the accelerations, and the joint torques with respect to $\mathit{q}$ , $\stackrel{.}{\mathit{q}}$ , $\stackrel{··}{\mathit{q}}$ are required. By utilizing the analytical formulations for manipulators (Park et al., 1995; Sohl and Bobrow, 2000), the motion optimization framework of Equation (1) was applied to a humanoid robot by Suleiman et al. (2008). Though the utilization can handle many types of motion optimization, there remain theoretical and practical issues in order to solve the motion optimization for humanoid systems.

First, the formulation should be extended for spherical joints, a free-floating base, or perhaps other types of joints in order to be applied to anthropomorphic systems because the formulations are originally for manipulators composed of rotational or translational actuators only. Though spherical joints or a free-floating base can be modeled by multiple rotational or translational joints, it often leads to the singularity problem of representing joint orientations. The link mass properties also need to be assigned to the corresponding multiple joints, which requires additional constraints on inertial properties and makes it difficult to perform dynamics analysis such as identification. The second issue is the computational complexity of the computation. The formulations in Suleiman et al. (2008) basically utilize the classical recursive formula of forward kinematics and inverse dynamics (Luh et al., 1980). The derivatives with respect to the coordinates of one joint are computed according to the recursive formula, and the same procedure is applied for every set of joint coordinates. Therefore, when computing the partial derivative of the variables of one link, the computational complexity is almost O ( $N_J^2$ ), where $N_J$ is the number of DOF.

In the field of computer animation, a similar framework of motion optimization has been applied to generate the motion of a human figure. Fang and Pollard (2003) presented an efficient method to compute the Jacobian of the total force and moment acting on the whole body with computational complexity O ( $N_J$ ). Their method utilizes the recursive equations about the total external force and moment acting on the subsystem of the kinematic tree, where the subsystem is constructed recursively by assembling the links from the leaf-side. Unfortunately, the formulation does not provide the Jacobian for the joint constraint forces or joint torques, because the total external force acting on the subsystem is not equal to the joint constraint force, except for the root link. For the optimization including any joint torques, an efficient formulation about the Jacobian for any joints with O ( $N_J$ ) is needed.

As mentioned above, one typical example of $\partial {\mathit{y}}_{i,j}∕\partial {\mathit{x}}_j$ is the basic Jacobian, and the computational cost of a standard basic Jacobian is O ( $N_J$ ). This paper introduces an $18\times 18$ matrix that can represent the forward kinematics computation including velocities and accelerations via simple chain products. The matrix has the same features as a $6\times 6$ transformation matrix that represents position and orientation, as discussed later. By utilizing this matrix together with the formulation of the basic Jacobians, the computation method for an arbitrary $\partial {\mathit{y}}_{i,j}∕\partial {\mathit{x}}_j$ is introduced in this paper. Specifically, the derivation procedure of COM Jacobian (Sugihara and Nakamura, 2002) is utilized for computation of the derivatives of the joint torques. In the formulation, several types of joints can also be handled, and the computational complexity is O ( $N_J$ ). The computation of the Jacobian matrices is detailed in section 4.

The direct computation of Jacobian matrices does not always lead to the fast optimization. The gradient computation of h can be accelerated by avoiding the direct computation of Jacobian matrices, as seen in an efficient inverse kinematics computation for large-DOF system (Ayusawa and Nakamura, 2012). The computational speed is improved by decomposing the gradient computation into the combination of the forward kinematics and inverse dynamics computation. An efficient computation of the gradient inspired by such a method is introduced insection 5.

3. Mathematical notations and comprehensive motion transformation matrix

This section presents the preliminary notation of variables in the paper, and introduces a useful matrix to represent the forward kinematics computation including velocities and accelerations of the multi-body system.

3.1. Definitions of basic geometric and mechanical variables

1. ${\mathit{O}}_n$ and ${\mathit{E}}_n$ are $n\times n$ zero and identity matrices respectively. ${\mathit{O}}_{n\times m}$ indicates a $n\times m$ zero matrix

2. The skew operator is represented as follows

   $$\begin{array}{c}\left[\mathit{x}\times \right]\triangleq \left[\begin{array}{ccc}0\hfill & -x_{\left(3\right)}\hfill & x_{\left(2\right)}\hfill \\ x_{\left(3\right)}\hfill & 0\hfill & -x_{\left(1\right)}\hfill \\ -x_{\left(2\right)}\hfill & x_{\left(1\right)}\hfill & 0\hfill \end{array}\right]\hfill \end{array}$$

3. The position and orientation matrix of the coordinate system of a rigid body are $\mathit{p}$ and $\mathit{R}$ , respectively.

4. Let $\mathit{\omega }$ and $\mathit{\nu }$ be the angular and linear velocities represented by the local coordinates, respectively. The following relationship holds

   $$\begin{array}{c}\stackrel{.}{\mathit{R}}=\mathit{R}\left[\mathit{\omega }\times \right]\hfill \\ \mathit{\nu }\triangleq {\mathit{R}}^T\stackrel{.}{\mathit{p}}\hfill \end{array}$$

   (3)

5. The linear and angular velocities are concatenated and defined as in the following vector of spatial velocity

   $$\begin{array}{c}\mathit{\upsilon }\triangleq \left[\begin{array}{c}\mathit{\nu }\hfill \\ \mathit{\omega }\hfill \end{array}\right]\hfill \end{array}$$

6. The $6\times 6$ spatial transformation matrix for spatial velocities is defined as follows

   $$\begin{array}{c}\mathit{A}\left(\mathit{p},\mathit{R}\right)\triangleq \left[\begin{array}{cc}\mathit{R}\hfill & \left[\mathit{p}\times \right]\mathit{R}\hfill \\ {\mathit{O}}_3\hfill & \mathit{R}\hfill \end{array}\right]\hfill \end{array}$$

7. Let us define the operator for the linear and angular velocities as follows

   $$\begin{array}{c}\left[{\mathit{\upsilon }}_{\bullet }\right]\triangleq \left[\begin{array}{cc}\left[\mathit{\omega }\times \right]\hfill & \left[\mathit{\nu }\times \right]\hfill \\ {\mathit{O}}_3\hfill & \left[\mathit{\omega }\times \right]\hfill \end{array}\right]\hfill \\ {\mathit{\upsilon }}_{1\bullet }{\mathit{\upsilon }}_2\triangleq \left[{\mathit{\upsilon }}_{1\bullet }\right]{\mathit{\upsilon }}_2\hfill \end{array}$$

8. The above operator satisfies the binary operation axioms in Appendix A.1. The following important relationships also hold

   $$\begin{array}{c}\stackrel{.}{\mathit{A}}=\mathit{A}\left[{\mathit{\upsilon }}_{\bullet }\right]\hfill \end{array}$$

   (4)
   $$\begin{array}{c}\mathit{A}\left[{\mathit{\upsilon }}_{\bullet }\right]{\mathit{A}}^{-1}=\left[(\mathit{A}\mathit{\upsilon }{)}_{\bullet }\right]\hfill \end{array}$$

   (5)

9. The inertial properties of a rigid body consist of mass $m$ , center of mass $\mathit{c}$ , and inertia tensor ${\mathit{I}}_c$ . They can be summarized by the following $6\times 6$ matrix

   $$\begin{array}{c}\mathit{M}\triangleq \left[\begin{array}{cc}m{\mathit{E}}_3\hfill & m{\left[\mathit{c}\times \right]}^T\hfill \\ m\left[\mathit{c}\times \right]\hfill & {\mathit{I}}_c+m\left[\mathit{c}\times \right]{\left[\mathit{c}\times \right]}^T\hfill \end{array}\right]\hfill \end{array}$$

10. Let the inertial forces of a rigid body be $\hat{\mathit{f}}$ and the moment around its coordinate be $\mathit{n}$ . They are represented in the global frame. Then, let us define a 6-axis force $\mathit{f}$ represented by the local coordinate as follows

    $$\begin{array}{c}\mathit{f}\triangleq \left[\begin{array}{c}{\mathit{R}}^T\hat{\mathit{f}}\hfill \\ {\mathit{R}}^T\mathit{n}\hfill \end{array}\right]\hfill \end{array}$$

11. The equations of motion of a rigid body are

    $$\begin{array}{c}\mathit{f}=\mathit{M}\stackrel{.}{\mathit{\upsilon }}-{\left[{\mathit{\upsilon }}_{\bullet }\right]}^T\mathit{M}\mathit{\upsilon }\hfill \end{array}$$

    (6)

    A variation of the above equation is written as follows by using matrix $\mathit{D}$

    $$\begin{array}{c}\delta \mathit{f}=\mathit{M}\delta \stackrel{.}{\mathit{\upsilon }}-\mathit{D}\delta \mathit{\upsilon }\hfill \\ \mathit{D}\triangleq -\left[(\mathit{M}\mathit{\upsilon }{)}_{\hat{\bullet }}\right]-{\left[{\mathit{\upsilon }}_{\bullet }\right]}^T\mathit{M}\hfill \end{array}$$

    (7)

12. Operation $\left[\hat{\bullet }\right]$ is defined as follows

$$\begin{array}{cc}\left[{\mathit{f}}_{\hat{\bullet }}\right]\hfill & \triangleq \left[\begin{array}{cc}{\mathit{O}}_3\hfill & \left[\hat{\mathit{f}}\times \right]\hfill \\ \left[\hat{\mathit{f}}\times \right]\hfill & \left[\mathit{n}\times \right]\hfill \end{array}\right]\end{array}$$

Note that the following relationship between [∙] and $\left[\hat{\bullet }\right]$ holds

$$\begin{array}{cc}{\left[{\hat{\mathit{f}}}_1\bullet \right]}^T{\hat{\mathit{f}}}_2\hfill & =\left[{\hat{\mathit{f}}}_{2\hat{\bullet }}\right]{\hat{\mathit{f}}}_1\end{array}$$

(8)

3.2. Comprehensive motion transformation matrix (CMTM)

Let us define the following new $18\times 18$ matrix $\mathit{X}$ and call it the CMTM

$$\begin{array}{c}\mathit{X}\left(\mathit{A},\mathit{\upsilon },\stackrel{.}{\mathit{\upsilon }}\right)\triangleq \left[\begin{array}{ccc}{\mathit{X}}_1\hfill & {\mathit{O}}_6\hfill & {\mathit{O}}_6\hfill \\ {\mathit{X}}_2\hfill & {\mathit{X}}_1\hfill & {\mathit{O}}_6\hfill \\ {\mathit{X}}_3\hfill & {\mathit{X}}_2\hfill & {\mathit{X}}_1\hfill \end{array}\right]\hfill \\ \triangleq \left[\begin{array}{ccc}\mathit{A}\hfill & {\mathit{O}}_6\hfill & {\mathit{O}}_6\hfill \\ \mathit{A}\left[{\mathit{\upsilon }}_{\bullet }\right]\hfill & \mathit{A}\hfill & {\mathit{O}}_6\hfill \\ \frac{1}{2}\mathit{A}\left(\left[{\stackrel{.}{\mathit{\upsilon }}}_{\bullet }\right]+{\left[{\mathit{\upsilon }}_{\bullet }\right]}^2\right)\hfill & \mathit{A}\left[{\mathit{\upsilon }}_{\bullet }\right]\hfill & \mathit{A}\hfill \end{array}\right]\end{array}$$

(9)

The following variation of the 18-dimensional vector is defined as follows

$$\begin{array}{c}\delta \mathit{x}\triangleq \left[\begin{array}{c}\delta \mathit{\alpha }\hfill \\ \delta \mathit{\upsilon }\hfill \\ \delta \stackrel{.}{\mathit{\upsilon }}\hfill \end{array}\right]\hfill \end{array}$$

(10)

where variation $\delta \mathit{\alpha }$ has the following relationship

$$\begin{array}{c}\left[(\delta \mathit{\alpha }{)}_{\bullet }\right]\triangleq {\mathit{A}}^{-1}\left(\delta \mathit{A}\right)\hfill \end{array}$$

Vector $\delta \mathit{x}$ is the concatenated vector of the variation of the standard 6-dimensional coordinates, velocities, and accelerations.

To handle the differential operation of matrix $\mathit{X}$ , the following variation of the 18-dimensional vector is newly defined as follows

$$\begin{array}{c}\delta \mathit{\xi }\triangleq \left[\begin{array}{c}\delta \mathit{\alpha }\hfill \\ \delta \mathit{\zeta }\hfill \\ \delta \mathit{\eta }\hfill \end{array}\right]\hfill \end{array}$$

(11)

where

$$\begin{array}{c}\delta \mathit{\zeta }\triangleq \delta \mathit{\upsilon }+\left[(\delta \mathit{\alpha }{)}_{\bullet }\mathit{\upsilon }\right]\hfill \\ \delta \mathit{\eta }\triangleq \frac{1}{2}\left(\delta \stackrel{.}{\mathit{\upsilon }}+\left[(\delta \mathit{\alpha }{)}_{\bullet }\stackrel{.}{\mathit{\upsilon }}\right]+\left[(\delta \mathit{\zeta }{)}_{\bullet }\mathit{\upsilon }\right]\right)\hfill \end{array}$$

For clarity, we summarize the above equations as follows

$$\begin{array}{c}\delta \mathit{\xi }=\mathit{S}\delta \mathit{x}\hfill \end{array}$$

(12)

Matrix $\mathit{S}$ transforms variation $\delta \mathit{x}$ into a new vector $\delta \mathit{\xi }$ , which is written as follows

$$\begin{array}{c}\mathit{S}\left(\mathit{\upsilon },\stackrel{.}{\mathit{\upsilon }}\right)\triangleq \left[\begin{array}{ccc}{\mathit{E}}_6\hfill & {\mathit{O}}_6\hfill & {\mathit{O}}_6\hfill \\ -\left[{\mathit{\upsilon }}_{\bullet }\right]\hfill & {\mathit{E}}_6\hfill & {\mathit{O}}_6\hfill \\ -\frac{1}{2}\left(\left[{\stackrel{.}{\mathit{\upsilon }}}_{\bullet }\right]-{\left[{\mathit{\upsilon }}_{\bullet }\right]}^2\right)\hfill & -\frac{1}{2}\left[{\mathit{\upsilon }}_{\bullet }\right]\hfill & \frac{1}{2}{\mathit{E}}_6\hfill \end{array}\right]\hfill \end{array}$$

(13)

The inverse matrix of $\mathit{S}$ always exists, and is computed as follows

$$\begin{array}{c}{\mathit{S}}^{-1}=\left[\begin{array}{ccc}{\mathit{E}}_6\hfill & {\mathit{O}}_6\hfill & {\mathit{O}}_6\hfill \\ \left[{\mathit{\upsilon }}_{\bullet }\right]\hfill & {\mathit{E}}_6\hfill & {\mathit{O}}_6\hfill \\ \left[{\stackrel{.}{\mathit{\upsilon }}}_{\bullet }\right]\hfill & \left[{\mathit{\upsilon }}_{\bullet }\right]\hfill & 2{\mathit{E}}_6\hfill \end{array}\right]\hfill \end{array}$$

Although the variation $\delta \mathit{x}$ is what we are familiar with in robotic analysis, its usage makes the forthcoming analysis of Jacobian matrices intractable. By using the newly defined variation $\delta \mathit{\xi }$ , the analysis becomes easier and clearer, as shown below. Once the Jacobian is derived, it can always be transformed back to $\delta \mathit{x}$ by via matrix $\mathit{S}$ .

Let us now define the following matrix and operator

$$\begin{array}{c}\left[\delta {\mathit{\xi }}^{\bullet }\right]\triangleq \left[\begin{array}{ccc}\left[\delta {\mathit{\alpha }}_{\bullet }\right]\hfill & {\mathit{O}}_6\hfill & {\mathit{O}}_6\hfill \\ \left[\delta {\mathit{\zeta }}_{\bullet }\right]\hfill & \left[\delta {\mathit{\alpha }}_{\bullet }\right]\hfill & {\mathit{O}}_6\hfill \\ \left[\delta {\mathit{\eta }}_{\bullet }\right]\hfill & \left[\delta {\mathit{\zeta }}_{\bullet }\right]\hfill & \left[\delta {\mathit{\alpha }}_{\bullet }\right]\hfill \end{array}\right]\hfill \\ \delta {{\mathit{\xi }}_1}^{\bullet }\delta {\mathit{\xi }}_2\triangleq \left[\delta {{\mathit{\xi }}_1}^{\bullet }\right]\delta {\mathit{\xi }}_2\hfill \end{array}$$

By utilizing the above operator, the following relationship holds

$$\begin{array}{c}\delta \mathit{X}=\mathit{X}\left[(\delta \mathit{\xi }{)}^{·}\right]\hfill \end{array}$$

(14)

It can be verified by computing each block matrix of $\delta {\mathit{X}}_i$

$$\begin{array}{c}\delta {\mathit{X}}_1=\delta \mathit{A}=\mathit{A}\left[\delta {\mathit{\alpha }}_{\bullet }\right]={\mathit{X}}_1\left[\delta {\mathit{\alpha }}_{\bullet }\right]\hfill \\ \delta {\mathit{X}}_2=\delta \mathit{A}\left[{\mathit{\upsilon }}_{\bullet }\right]+\mathit{A}\left[\delta {\mathit{\upsilon }}_{\bullet }\right]={\mathit{X}}_1\left[\delta {\mathit{\zeta }}_{\bullet }\right]+{\mathit{X}}_2\left[\delta {\mathit{\alpha }}_{\bullet }\right]\hfill \\ \delta {\mathit{X}}_3=\frac{1}{2}\delta \mathit{A}\left(\left[{\stackrel{.}{\mathit{\upsilon }}}_{\bullet }\right]+{\left[{\mathit{\upsilon }}_{\bullet }\right]}^2\right)\hfill \\ +\frac{1}{2}\mathit{A}\left(\left[\delta {\stackrel{.}{\mathit{\upsilon }}}_{\bullet }\right]+\left[\delta {\mathit{\upsilon }}_{\bullet }\right]\left[{\mathit{\upsilon }}_{\bullet }\right]+\left[{\mathit{\upsilon }}_{\bullet }\right]\left[\delta {\mathit{\upsilon }}_{\bullet }\right]\right)\\ ={\mathit{X}}_1\left[\delta {\mathit{\eta }}_{\bullet }\right]+{\mathit{X}}_2\left[\delta {\mathit{\zeta }}_{\bullet }\right]+{\mathit{X}}_3\left[\delta {\mathit{\alpha }}_{\bullet }\right]\end{array}$$

Equation (14) has the same form as Equation (3) or Equation (4). Actually, operator $\delta {{\mathit{\zeta }}_1}^{\bullet }\delta {\mathit{\zeta }}_2$ satisfies the binary operation axioms in Appendix A.1, which can be easily verified. In addition, the following equation also holds

$$\begin{array}{c}\mathit{X}\left[\delta {\mathit{\zeta }}^{\bullet }\right]{\mathit{X}}^{-1}=\left[(\mathit{X}\delta \mathit{\zeta }{)}^{\bullet }\right]\hfill \end{array}$$

(15)

Note that Equation (15) corresponds with Equation (5).

The set of matrix $\mathit{X}$ and operator ${(}^{\bullet }$ ) has similar mathematical features as matrix $\mathit{A}$ and ${(}_{\bullet }$ ) (i.e. the set of the adjoint map and Lie bracket operator). This means that many formulas of the kinematics operations on the position and orientation can be replaced with those operating on velocities and accelerations. Therefore, matrix $\mathit{X}$ can comprehensively handle the kinematics transformation for motion.

3.3. Definition and formulas of kinematics chain

This subsection presents the notation for open kinematic chain, and important formulas for the kinematics and dynamics.

1. The kinematic chain is tree-structured, and the indices are chosen from the base link toward the end of branches.

2. $p\left(i\right)$ is the index of a root-side link connected to link i.

3. $C\left(i\right)$ is the set of indices of leaves-side links connected to link i.

4. $\mathcal{C}\left(i\right)$ is the set of all leaves-side links recursively connected to link i.

5. $\mathcal{P}\left(i\right)$ is the set of all root-side links recursively connected to link i.

6. Let us define the following sets $\widehat{\mathcal{P}}\left(i\right)\triangleq \left\{i,\mathcal{P}\left(i\right)\right\}$ , $\widehat{\mathcal{C}}\left(i\right)\triangleq \left\{i,\mathcal{C}\left(i\right)\right\}$ .

7. $s\left(j,i\right)$ is the following selection function

   $$\begin{array}{c}s\left(j,i\right)\triangleq \left\{\begin{array}{cc}1\hfill & \left(i\in \widehat{\mathcal{P}}\left(j\right)\right)\hfill \\ 0\hfill & \left(\mathrm{others}\right)\hfill \end{array}\right.\hfill \end{array}$$

8. Let us represent the quantity $\mathit{y}$ of link i such that $\mathit{y}i$ .

9. Let us denote ${\mathit{y}}_j^i$ as the relative variable of $\mathit{y}$ from link i to j.

10. ${\mathit{q}}_i$ is the $n_{\mathrm{Ji}}$ sets of joint variables (angles), where $n_j$ is the number of DOFs of joint i, and the following relationship holds between the joint variables and the relative velocities between link i and $p\left(i\right)$

    $$\begin{array}{c}{\mathit{A}}_i^{p\left(i\right)}=e^{\left[({\mathit{K}}_i{\mathit{q}}_i{)}_{\bullet }\right]}\hfill \end{array}$$

    (16)

11. Matrix ${\mathit{K}}_i$ is a $6\times n_{\mathrm{Ji}}$ constant matrix defined according to the type of joint i. For instance, if joint i₁ is a rotational joint, joint i₂ is a translational one, joint i₃ is a spherical one and joint i₄ is a free floating one (6-DOF), the corresponding matrices are as follows

    $$\begin{array}{cc}{\mathit{K}}_{i_1}\triangleq {\left[\begin{array}{cc}{0_3}^T\hfill & {e_{i1}}^T\hfill \end{array}\right]}^T\hfill & \left(\mathrm{rotational}\right)\hfill \\ {\mathit{K}}_{i_2}\triangleq {\left[\begin{array}{cc}{e_{i2}}^T\hfill & {0_3}^T\hfill \end{array}\right]}^T\hfill & \left(\mathrm{translational}\right)\hfill \\ {\mathit{K}}_{i_3}\triangleq \left[\begin{array}{c}{\mathit{O}}_3\hfill \\ {\mathit{E}}_3\hfill \end{array}\right]\hfill & \left(\mathrm{spherical}\right)\hfill \\ {\mathit{K}}_{i_4}\triangleq {\mathit{E}}_6\hfill & \left(\mathrm{freefloating}\right)\hfill \end{array}$$

    where $e_{i1}$ and $e_{i2}$ mean the corresponding joint axis direction and $0_3$ is a 3-dimensional zero vector.

12. Vector $\delta {\mathit{\theta }}_i$ is a variance defined in the tangent vector space of ${\mathit{A}}_i^{p\left(i\right)}$ , which has the following differential relationship

    $$\begin{array}{c}\delta {\mathit{A}}_i^{p\left(i\right)}={\mathit{A}}_i^{p\left(i\right)}\left[\delta {{\mathit{\alpha }}_i^{p\left(i\right)}}_{\bullet }\right]\hfill \\ ={\mathit{A}}_i^{p\left(i\right)}\left[({\mathit{K}}_i\delta {\mathit{\theta }}_i{)}_{\bullet }\right]\end{array}$$

    (17)

    Note that, in the case of spherical joints or free floating joints, the tangent vector $\delta {\mathit{\theta }}_i$ is not equal to the variation of joint variable $\delta {\mathit{q}}_i$ due to Equation (17); for example, the angular velocity of a spherical joint is not equal to the derivative of the angle-axis vector representing the joint orientation.

13. Vector ${\mathit{\psi }}_i$ represents the joint velocity variables, and the following equations hold between ${\mathit{\psi }}_i$ and the relative coordinates

    $$\begin{array}{c}{\mathit{\upsilon }}_i^{p\left(i\right)}={\mathit{K}}_i{\mathit{\psi }}_i\hfill \end{array}$$

    (18)

    ${\mathit{\psi }}_i$ is not equal to $d{\mathit{\theta }}_i∕\mathrm{dt}$ .

14. Vector ${\mathit{f}}_j^{p\left(j\right)}$ denotes the constraint force of joint j, which has the following relationship with inertial force ${\mathit{f}}_j$ of link j and its links connected to the leaves-side

    $$\begin{array}{c}{\mathit{f}}_j^{p\left(j\right)}={\mathit{f}}_j+\sum _{k\in C\left(j\right)}{{\mathit{A}}_k^j}^{-T}{\mathit{f}}_k^j\hfill \end{array}$$

    (19)

    where $j=p\left(k\right)$ holds due to $k\in C\left(j\right)$ . The above recursive formula can be transformed into the following summation formula

    $$\begin{array}{c}{\mathit{f}}_j^{p\left(j\right)}=\sum _{k\in \widehat{\mathcal{C}}\left(j\right)}{{\mathit{A}}_k^j}^{-T}{\mathit{f}}_k\hfill \end{array}$$

    (20)

15. Vector ${\mathit{\tau }}_j$ represents the $n_{\mathrm{Ji}}$ dimensional vector of joint torque and can be extracted from the 6-dimensional vector of joint constraint forces ${\mathit{f}}_j^{p\left(j\right)}$ as follows

$$\begin{array}{c}{\mathit{\tau }}_j={\mathit{K}}_j^T{\mathit{f}}_j^{p\left(j\right)}\hfill \end{array}$$

(21)

3.4. CMTMs in the kinematic chain

Let us consider the following chain product of CMTMs

$$\begin{array}{c}{\mathit{X}}_j={\mathit{X}}_i{\mathit{X}}_j^i\hfill \end{array}$$

(22)

The chain products of CMTMs in Equation (22) represent the forward kinematics computation including differential kinematics for the velocity and acceleration as follows

$$\begin{array}{c}{\mathit{A}}_i={\mathit{A}}_i{\mathit{A}}_j^i\hfill \end{array}$$

(23)

$$\begin{array}{c}{\mathit{\upsilon }}_j={\mathit{\upsilon }}_j^i+{{\mathit{A}}_j^i}^{-1}{\mathit{\upsilon }}_i\hfill \end{array}$$

(24)

$$\begin{array}{c}{\stackrel{.}{\mathit{\upsilon }}}_j={\stackrel{.}{\mathit{\upsilon }}}_j^i+{{\mathit{A}}_j^i}^{-1}{\stackrel{.}{\mathit{\upsilon }}}_i+({{\mathit{A}}_j^i}^{-1}{\mathit{\upsilon }}_i{)}_{\bullet }{\mathit{\upsilon }}_j^i\hfill \end{array}$$

(25)

Proof :

The above relationships can be easily verified by writing the components of ${\mathit{X}}_i$ as follows

$$\begin{array}{c}{\mathit{X}}_{1j}={\mathit{X}}_{1i}{{\mathit{X}}_1}_j^i\hfill \end{array}$$

(26)

$$\begin{array}{c}{\mathit{X}}_{2}j={\mathit{X}}_{1i}{{\mathit{X}}_2}_j^i+{\mathit{X}}_{2i}{{\mathit{X}}_1}_j^i\hfill \end{array}$$

(27)

$$\begin{array}{c}{\mathit{X}}_{3}j={\mathit{X}}_{3}i{{\mathit{X}}_1}_j^i+{\mathit{X}}_{2i}{{\mathit{X}}_2}_j^i+{\mathit{X}}_{1i}{{\mathit{X}}_3}_j^i\hfill \end{array}$$

(28)

Now let us examine Equation (26), Equation (27), and Equation (28). From Equation (9), Equation (26) can be transformed into

$$\begin{array}{c}{\mathit{A}}_i={\mathit{A}}_i{\mathit{A}}_j^i\hfill \end{array}$$

Therefore, Equation (26) is equivalent to Equation (23): the standard forward kinematics operation.

Next, Equation (27) can be converted into

$$\begin{array}{c}{\mathit{A}}_j\left[{{\mathit{\upsilon }}_j}_{\bullet }\right]={\mathit{A}}_i{\mathit{A}}_j^i\left[{{\mathit{\upsilon }}_j^i}_{\bullet }\right]+{\mathit{A}}_i\left[{{\mathit{\upsilon }}_i}_{\bullet }\right]{\mathit{A}}_j^i\hfill \\ =\mathit{A}j\left[({\mathit{\upsilon }}_j^i+{{\mathit{A}}_j^i}^{-1}{\mathit{\upsilon }}_i{)}_{\bullet }\right]\end{array}$$

From the above equation, the following must also hold

$$\begin{array}{c}{\mathit{\upsilon }}_j={\mathit{\upsilon }}_j^i+{{\mathit{A}}_j^i}^{-1}{\mathit{\upsilon }}_i\hfill \end{array}$$

Therefore, Equation (27) is equal to Equation (24), i.e. the differential forward kinematics operation about linear and angular velocities.

Finally, Equation (28) can be written as follows

$$\begin{array}{c}\frac{1}{2}{\mathit{A}}_j\left(\left[{{\stackrel{.}{\mathit{\upsilon }}}_j}_{\bullet }\right]+{\left[{{\mathit{\upsilon }}_j}_{\bullet }\right]}^2\right)\\ =\frac{1}{2}{\mathit{A}}_i\left(\left[{\stackrel{.}{\mathit{\upsilon }}}_i\bullet \right]+{\left[{{\mathit{\upsilon }}_i}_{\bullet }\right]}^2\right){\mathit{A}}_j^i+{\mathit{A}}_i\left[{{\mathit{\upsilon }}_i}_{\bullet }\right]{\mathit{A}}_j^i\left[{{\mathit{\upsilon }}_j^i}_{\bullet }\right]\hfill \\ +\frac{1}{2}{\mathit{A}}_i{\mathit{A}}_j^i\left(\left[{\stackrel{.}{\mathit{\upsilon }}}_j^i\bullet \right]+{\left[{{\mathit{\upsilon }}_j^i}_{\bullet }\right]}^2\right)\\ =\frac{1}{2}{\mathit{A}}_j\left(\left[({\stackrel{.}{\mathit{\upsilon }}}_j^i+{{\mathit{A}}_j^i}^{-1}{\stackrel{.}{\mathit{\upsilon }}}_i+({{\mathit{A}}_j^i}^{-1}{\mathit{\upsilon }}_i{)}_{\bullet }{\mathit{\upsilon }}_j^i{)}_{\bullet }\right]+{\left[{{\mathit{\upsilon }}_j}_{\bullet }\right]}^2\right)\hfill \end{array}$$

Therefore, the following must hold

$$\begin{array}{c}{\stackrel{.}{\mathit{\upsilon }}}_j={\stackrel{.}{\mathit{\upsilon }}}_j^i+{{\mathit{A}}_j^i}^{-1}{\stackrel{.}{\mathit{\upsilon }}}_i+({{\mathit{A}}_j^i}^{-1}{\mathit{\upsilon }}_i{)}_{\bullet }{\mathit{\upsilon }}_j^i\hfill \end{array}$$

Then, Equation (28) is transformed into Equation (25), i.e. the differential forward kinematics operation for the linear and angular accelerations.

     □

From the above, it is apparent that the chain products of CMTMs in Equation (22) represent the standard and differential kinematics computation of a kinematic chain. The formulation using CMTM can therefore handle the kinematics operations in a comprehensive manner. This is a strong advantage when introducing arbitrary Jacobian matrices in the next section.

Now let us define the following variation of $3n_{\mathrm{Ji}}$ dimensional vector consisting of the variations of the joint variable, velocity, and its derivative

$$\begin{array}{c}\delta \mathit{\chi }\triangleq \left[\begin{array}{c}\delta \mathit{\theta }\hfill \\ \delta \mathit{\psi }\hfill \\ \delta \stackrel{.}{\mathit{\psi }}\hfill \end{array}\right]\hfill \end{array}$$

(29)

According to Equation (11), Equation (17), and Equation (18), the relationship between $\delta {\mathit{x}}_i^{p\left(i\right)}$ and ${\delta }_{{\mathit{\chi }}_i}$ is summarized as follows

$$\begin{array}{c}\delta {\mathit{x}}_i^{p\left(i\right)}={\mathit{G}}_i{\delta }_{{\mathit{\chi }}_i}\hfill \end{array}$$

(30)

where

$$\begin{array}{c}\mathit{G}i\triangleq \left[\begin{array}{ccc}{\mathit{K}}_i\hfill & {\mathit{O}}_{6\times \mathrm{nJi}}\hfill & {\mathit{O}}_{6\times \mathrm{nJi}}\hfill \\ {\mathit{O}}_{6\times \mathrm{nJi}}\hfill & {\mathit{K}}_i\hfill & {\mathit{O}}_{6\times \mathrm{nJi}}\hfill \\ {\mathit{O}}_{6\times \mathrm{nJi}}\hfill & {\mathit{O}}_{6\times \mathrm{nJi}}\hfill & {\mathit{K}}_i\hfill \end{array}\right]\hfill \end{array}$$

The relationships between the variables of the link and joint coordinates and their variations are summarized in Figure 1.

Fig. 1.

Overview of the relationships between the variables used in the paper.

4. Computation of arbitrary Jacobians

This section shows the different types of arbitrary Jacobian matrices used in Equation (2) by utilizing CMTM. The formulation in this paper are strictly speaking not Jacobian matrices as in the case of the basic Jacobian. The basic Jacobian is the coefficient matrix in the linear differential relationship between the joint angle velocities and the linear/angular velocities of the corresponding link. Since the integration of angular velocity has no physical meaning, the corresponding part of the basic Jacobian is not equivalent to the partial derivatives of the orientation variable. Several Jacobians introduced in this section also mean the coefficient matrices in the linear differential relationship between the variation of joint variables $\delta {\mathit{x}}_j$ and the variation of arbitrary physical quantities $\delta {\mathit{y}}_{i,j}$ . However, in this paper, we will refer to them as Jacobians for descriptive purposes.

4.1. Jacobians of link posture, velocity and acceleration

Let us compute matrix ${\mathit{J}}_j$ that converts the variation ${\delta }_{{\mathit{\chi }}_{\mathrm{all}}}$ of all joints to variation $\delta {\mathit{x}}_j$ for link j as follows

$$\begin{array}{c}\delta {\mathit{x}}_j={\mathit{J}}_j{\delta }_{{\mathit{\chi }}_{\mathrm{all}}}=\sum _k{\mathit{J}}_{(j,k)}{\delta }_{{\mathit{\chi }}_k}\hfill \end{array}$$

(31)

where, ${\mathit{J}}_{(j,k)}$ is the block matrix of ${\mathit{J}}_j$ related to joint k.

The matrix ${\mathit{J}}_{(j,k)}$ can be directly computed as follows

$$\begin{array}{c}{\mathit{J}}_{(j,k)}=s_{(j,k)}{\hat{\mathit{X}}}_k^j{\mathit{G}}_k\hfill \end{array}$$

(32)

where

$$\begin{array}{c}{\hat{\mathit{X}}}_k^j\triangleq {\mathit{S}}_j^{-1}{\mathit{X}}_k^j{\mathit{S}}_k^{p\left(k\right)}\hfill \end{array}$$

(33)

This subsection contains the proof that Equation (32) holds. The Jacobian ${\mathit{J}}_j$ is analogous to the basic Jacobian (Khatib, 1987) as shown in Figure 2.

Fig. 2.

The relationship between basic Jacobian and the variance defined with CMTM. The new framework can also be applied to free-floating and spherical joints.

As mentioned in the previous section, matrix $\mathit{X}$ has the same features as $\mathit{A}$ . Matrix ${\mathit{J}}_j$ can be computed in a similar manner when computing basic Jacobians as the following proof.

Proof :

Let us consider ${\mathit{X}}_j$ of link j. The following chain products hold among CMTMs

$$\begin{array}{c}{\mathit{X}}_j={\mathit{X}}_{p\left(k\right)}{\mathit{X}}_k^{p\left(k\right)}{\mathit{X}}_j^k\left(k\in \widehat{\mathcal{P}}\left(j\right)\right)\hfill \end{array}$$

Then, the variation of ${\mathit{X}}_j$ can be computed according to the above chain products, and we have the following

$$\begin{array}{c}\delta {\mathit{X}}_j=\sum _{k\in \widehat{\mathcal{P}}\left(j\right)}{\mathit{X}}_{p\left(k\right)}\delta {\mathit{X}}_k^{p\left(k\right)}{\mathit{X}}_j^k\hfill \end{array}$$

By utilizing Equation (14) and Equation (15), the above equation can be transformed into

$$\begin{array}{c}{\mathit{X}}_j\left[(\delta {\mathit{\xi }}_j{)}^{\bullet }\right]=\sum _{k\in \widehat{\mathcal{P}}\left(j\right)}{\mathit{X}}_{p\left(k\right)}{\mathit{X}}_k^{p\left(k\right)}\left[(\delta {\mathit{\xi }}_k^{p\left(k\right)}{)}^{\bullet }\right]{\mathit{X}}_j^k\hfill \\ ={\mathit{X}}_j\sum _{k\in \widehat{\mathcal{P}}\left(j\right)}\left[({\mathit{X}}_k^j\delta {\mathit{\xi }}_k^{p\left(k\right)}{)}^{\bullet }\right]\end{array}$$

According to the above equation, the following equation also holds

$$\begin{array}{c}\delta {\mathit{\xi }}_j=\sum _{k\in \widehat{\mathcal{P}}\left(j\right)}{\mathit{X}}_k^j\delta {\mathit{\xi }}_k^{p\left(k\right)}\hfill \end{array}$$

(34)

The coefficient matrix of Equation (34) represents the Jacobian matrix with respect to $\delta \mathit{\xi }$ , and each block matrix is equal to relative CMTM ${\mathit{\xi }}_k^{p\left(k\right)}$ .

Because the desired Jacobian matrix is with respect to $\delta {\mathit{x}}_j$ , let us compute it by transforming variations of Equation (34) from $\delta \mathit{\xi }$ to $\delta \mathit{x}$ and from $\delta \mathit{x}$ to $\delta \mathit{\chi }$ .

First, by utilizing Equation (12), the following equation is obtained

$$\begin{array}{c}\delta {\mathit{x}}_j=\sum _{k\in \widehat{\mathcal{P}}\left(j\right)}{\mathit{S}}_j^{-1}{\mathit{X}}_k^j{\mathit{S}}_k^{p\left(k\right)}\delta {\mathit{x}}_k^{p\left(k\right)}\hfill \\ =\sum _k{s}_{(j,k)}{\hat{\mathit{X}}}_k^j\delta {\mathit{x}}_k^{p\left(k\right)}\end{array}$$

The next transformation can be performed with Equation (30) as follows

$$\begin{array}{c}\delta {\mathit{x}}_j=\sum _k{s}_{(j,k)}{\hat{\mathit{X}}}_k^j{\mathit{G}}_k{\delta }_{{\mathit{\chi }}_k}\hfill \end{array}$$

(35)

From Equation (35), the Jacobian matrix shown in Equation (31) was finally derived as given in Equation (32).

□

The computation of ${\mathit{J}}_j$ in Equation (31) requires ${\mathit{J}}_{(j,k)}$ for all k. Each ${\mathit{J}}_{(j,k)}$ can be obtained by the matrix products according to Equation (32) and Equation (33). Therefore, the computational complexity of ${\mathit{J}}_j$ is O( $N_J$ ). Because the direct computation of $18\times 18$ matrix products in Equation (33) is computationally inefficient, the solution of $6\times 6$ block matrices in ${\hat{\mathit{X}}}_j^i$ is given in Appendix A.2.

4.2. Jacobians of link inertial forces

This subsection derives matrix ${\mathit{L}}_j$ that converts variation ${\delta }_{{\mathit{\chi }}_{\mathrm{all}}}$ of all joints to force variation $\delta {\mathit{f}}_j$ of link j as follows

$$\begin{array}{c}\delta {\mathit{f}}_j={\mathit{L}}_j{\delta }_{{\mathit{\chi }}_{\mathrm{all}}}=\sum _k{\mathit{L}}_{(j,k)}{\delta }_{{\mathit{\chi }}_k}\hfill \end{array}$$

(36)

where, ${\mathit{L}}_{(j,k)}$ is the block matrix of ${\mathit{L}}_j$ related to joint k.

The matrix ${\mathit{L}}_{(j,k)}$ can be computed as follows

$$\begin{array}{c}{\mathit{L}}_{(j,k)}={\mathit{H}}_j{\mathit{J}}_{(j,k)}\hfill \end{array}$$

(37)

where

$$\begin{array}{cc}{\mathit{H}}_j\hfill & \triangleq \left[\begin{array}{ccc}{\mathit{O}}_6\hfill & {\mathit{D}}_j\hfill & {\mathit{M}}_j\hfill \end{array}\right]\end{array}$$

This subsection verifies Equation (37).

Proof :

Let us first consider the variation of equations of motion of link j. According to Equation (7), the following equations are obtained

$$\begin{array}{cc}\delta {\mathit{f}}_j\hfill & ={\mathit{H}}_j\delta {\mathit{x}}_j\end{array}$$

From Equation (31), the above equation can be also transformed into the following

$$\begin{array}{c}\delta {\mathit{f}}_j=\sum _k{\mathit{H}}_j{\mathit{J}}_{(j,k)}{\delta }_{{\mathit{\chi }}_k}\hfill \end{array}$$

(38)

Therefore, the Jacobian matrix shown in Equation (36) could be derived as Equation (37).

□

For the sake of the subsequent discussions, let us transform Equation (38) to a linear form with respect to $\delta {\mathit{\xi }}_j$ by using Equation (12)

$$\begin{array}{c}\delta {\mathit{f}}_j={\mathit{H}}_j{\mathit{S}}_j^{-1}\delta {\mathit{\xi }}_j\hfill \end{array}$$

(39)

4.3. Jacobians of joint constraint forces

In this subsection, we compute matrix ${\mathit{N}}_j$ that converts the variations ${\delta }_{{\mathit{\chi }}_{\mathrm{all}}}$ of all joints to force variation $\delta {\mathit{f}}_j^{p\left(j\right)}$ of link j as follows

$$\begin{array}{c}\delta {\mathit{f}}_j^{p\left(j\right)}={\mathit{N}}_j{\delta }_{{\mathit{\chi }}_{\mathrm{all}}}=\sum _k{\mathit{L}}_{(j,k)}{\delta }_{{\mathit{\chi }}_k}\hfill \end{array}$$

(40)

where, ${\mathit{N}}_{(j,k)}$ is the block matrix of ${\mathit{N}}_j$ related to joint k.

The matrix ${\mathit{N}}_{(j,k)}$ can be computed as follows

$$\begin{array}{c}{\mathit{N}}_{(j,k)}=\left\{\begin{array}{cc}{\hat{\mathit{H}}}_j{\mathit{J}}_{(j,k)}\hfill & \left(k\in \widehat{\mathcal{P}}\left(j\right)\right)\hfill \\ {{\mathit{A}}_k^j}^{-T}\left({\hat{\mathit{H}}}_k{\mathit{J}}_{\left(k,k\right)}-\left[{{\mathit{f}}_k^{p\left(k\right)}}_{\hat{\bullet }}\right]\mathit{T}{\mathit{G}}_k\right)\hfill & \left(k\in \mathcal{C}\left(j\right)\right)\hfill \\ {\mathit{O}}_{6\times 18}\hfill & \left(\mathrm{others}\right)\hfill \end{array}\right.\hfill \end{array}$$

(41)

where, matrix ${\hat{\mathit{H}}}_j$ can be recursively computed from leaf-side links as follows

$$\begin{array}{c}{\hat{\mathit{H}}}_j={\mathit{H}}_j+\sum _{k\in C\left(j\right)}{{\mathit{A}}_k^j}^{-T}{\hat{\mathit{H}}}_k{\mathit{S}}_k^{-1}{\mathit{X}}_j^k{\mathit{S}}_j\hfill \end{array}$$

(42)

Additionally, matrix $\mathit{T}$ is defined as follows

$$\begin{array}{c}\mathit{T}\triangleq \left[\begin{array}{ccc}{\mathit{E}}_6\hfill & {\mathit{O}}_6\hfill & {\mathit{O}}_6\hfill \end{array}\right]\hfill \end{array}$$

Let us verify Equation (41) in this subsection.

Proof :

From Equation (19), the following equation about the joint constraint force ${\mathit{f}}_j^{p\left(j\right)}$ can be obtained

$$\begin{array}{c}\delta {\mathit{f}}_j^{p\left(j\right)}=\delta {\mathit{f}}_j+\sum _{k\in C\left(j\right)}\left({{\mathit{A}}_k^{p\left(k\right)}}^{-T}\delta {\mathit{f}}_k^{p\left(k\right)}+\delta {{\mathit{A}}_k^{p\left(k\right)}}^{-T}{\mathit{f}}_k^{p\left(k\right)}\right)\hfill \end{array}$$

According to Equation (39), the above equation can be transformed into

$$\begin{array}{c}\delta {\mathit{f}}_j^{p\left(j\right)}=\left({\mathit{H}}_j{\mathit{S}}_j^{-1}\right)\delta {\mathit{\xi }}_j\hfill \\ +\sum _{k\in C\left(j\right)}\left({{\mathit{A}}_k^{p\left(k\right)}}^{-T}\delta {\mathit{f}}_k^{p\left(k\right)}+\delta {{\mathit{A}}_k^{p\left(k\right)}}^{-T}{\mathit{f}}_k^{p\left(k\right)}\right)\end{array}$$

(43)

On the other hand, the following equation holds from Equation (34)

$$\begin{array}{c}\delta {\mathit{\xi }}_j={\mathit{X}}_{p\left(j\right)}^j\left(\sum _{l\in \widehat{\mathcal{P}}\left(p\left(j\right)\right)}{\mathit{X}}_l^{p\left(j\right)}\delta {\mathit{\xi }}_l^{p\left(l\right)}\right)+\delta {\mathit{\xi }}_j^{p\left(j\right)}\hfill \\ ={\mathit{X}}_{p\left(j\right)}^j\delta {\mathit{\xi }}_{p\left(j\right)}+\delta {\mathit{\xi }}_j^{p\left(j\right)}\end{array}$$

(44)

Because ${\mathit{A}}^{-T}$ and $\mathit{X}$ are transformation matrices, Equation (44) and Equation (43) have the same form as Equation (71) and Equation (72) given in Appendix A.3, respectively. According to the similar derivations of Equation (73), Equation (74), Equation (75) in Appendix A.3, the following recursive formula can be obtained

$$\begin{array}{c}\delta {\mathit{f}}_j^{p\left(j\right)}={\hat{\mathit{H}}}_j\delta {\mathit{x}}_j+{\hat{\mathit{h}}}_j\left(={\hat{\mathit{H}}}_j{\mathit{S}}_j^{-1}\delta {\mathit{\xi }}_j+{\hat{\mathit{h}}}_j\right)\hfill \end{array}$$

(45)

where

$$\begin{array}{c}{\hat{\mathit{h}}}_j=\sum _{k\in C\left(j\right)}\left({{\mathit{A}}_k^j}^{-T}{\hat{\mathit{H}}}_k{\mathit{S}}_k^{-1}\delta {\mathit{\xi }}_k^{p\left(k\right)}\right.\hfill \\ \left.+\delta {{\mathit{A}}_k^{p\left(k\right)}}^{-T}{\mathit{f}}_k^{p\left(k\right)}+{{\mathit{A}}_k^j}^{-T}{\hat{\mathit{h}}}_k\right)\end{array}$$

(46)

It should be noted that, if the set of ${\mathit{A}}^{-T}$ and $\mathit{X}$ is replaced with ${\mathit{A}}^{-T}$ and $\mathit{A}$ and if there exists no bias terms such as $\delta {\mathit{\xi }}_j^{p\left(j\right)}$ , the transformation based on Appendix A.3 has the same formula when introducing the linear and angular momentum Jacobian.

Let us expand the term of ${{\mathit{A}}_k^j}^{-T}{\hat{\mathit{h}}}_k$ in Equation (46) by its recursive computation from the base-link toward the the end-links. In addition, by using the transformation from $\delta \mathit{\xi }$ to $\delta \mathit{x}$ with Equation (12), Equation (46) can be transformed into

$$\begin{array}{c}{\hat{\mathit{h}}}_j=\sum _{k\in \mathcal{C}\left(j\right)}\left({{\mathit{A}}_k^j}^{-T}{\hat{\mathit{H}}}_k{\mathit{S}}_k^{-1}{\mathit{S}}_k^{p\left(k\right)}\delta {\mathit{x}}_k^{p\left(k\right)}\right.\\ \left.+{{\mathit{A}}_{p\left(k\right)}^j}^{-T}\delta {{\mathit{A}}_k^{p\left(k\right)}}^{-T}{\mathit{f}}_k^{p\left(k\right)}\right)\end{array}$$

There also exists the following conversion of variation $\delta {{\mathit{A}}_k^{p\left(k\right)}}^{-T}$

$$\begin{array}{c}\delta {{\mathit{A}}_k^{p\left(k\right)}}^{-T}{\mathit{f}}_k^{p\left(k\right)}=-{{\mathit{A}}_k^{p\left(k\right)}}^{-T}{\left[\delta {\mathit{\alpha }}_k^{p\left(k\right)}\bullet \right]}^T{\mathit{f}}_k^{p\left(k\right)}\hfill \\ =-{{\mathit{A}}_k^{p\left(k\right)}}^{-T}\left[{\mathit{f}}_k^{p\left(k\right)}\hat{\bullet }\right]\delta {\mathit{\alpha }}_k^{p\left(k\right)}\end{array}$$

According to the above equation, ${\hat{\mathit{h}}}_j$ can be written as:

$$\begin{array}{c}{\hat{\mathit{h}}}_j=\sum _{k\in \mathcal{C}\left(j\right)}\left({{\mathit{A}}_k^j}^{-T}{\hat{\mathit{H}}}_k{\hat{\mathit{X}}}_{\left(k,k\right)}-{{\mathit{A}}_k^j}^{-T}\left[{\mathit{f}}_k^{p\left(k\right)}\hat{\bullet }\right]\mathit{T}\right)\delta {\mathit{x}}_k^{p\left(k\right)}\hfill \end{array}$$

(47)

Note that, ${\hat{\mathit{X}}}_{\left(k,k\right)}={\mathit{S}}_k^{-1}{\mathit{S}}_k^{p\left(k\right)}$ is from Equation (33).

By substituting Equation (42) and Equation (47) for Equation (45), and by converting the variations from $\delta {\mathit{x}}_k^{p\left(k\right)}$ to $\delta {\mathit{\chi }}_k^{p\left(k\right)}$ , the following equation holds

$$\begin{array}{c}\delta {\mathit{f}}_j^{p\left(j\right)}=\sum _{k\in \widehat{\mathcal{P}}\left(j\right)}{\hat{\mathit{H}}}_j{\mathit{J}}_{(j,k)}\delta {\mathit{\chi }}_k^{p\left(k\right)}\\ +\sum _{k\in \mathcal{C}\left(j\right)}{{\mathit{A}}_k^j}^{-T}\left({\hat{\mathit{H}}}_k{\mathit{J}}_{\left(k,k\right)}-\left[{\mathit{f}}_k^{p\left(k\right)}\hat{\bullet }\right]\mathit{T}{\mathit{G}}_k\right)\delta {\mathit{\chi }}_k^{p\left(k\right)}\end{array}$$

(48)

From Equation (48), the Jacobian matrix shown in Equation (40) can be finally derived as given in Equation (41).

□

The important advancement from the conventional formulation (Suleiman et al., 2008) is the recursive formula of inertial matrices in Equation (42), which achieves significant improvement in computational efficiency. The computation of Equation (41) requires matrix ${\hat{\mathit{H}}}_j$ . It means that ${\hat{\mathit{H}}}_j$ for all link j needs to be computed in advance according to recursive formula Equation (42). This computational complexity to update ${\hat{\mathit{H}}}_j$ for all links is O( $N_J$ ). After updating ${\hat{\mathit{H}}}_j$ , matrix ${\mathit{N}}_{(j,k)}$ for any j and k can be directly computed by Equation (41). The computation of ${\mathit{N}}_j$ requires ${\mathit{N}}_{(j,k)}$ for all k. Therefore, the computational complexity of ${\mathit{N}}_j$ including recursive computation of Equation (42) is O( $N_J$ ).

The direct computation of Equation (42) with $18\times 18$ matrices is computationally inefficient. The final form of the $6\times 18$ matrix ${\hat{\mathit{H}}}_j$ derived from Equation (42) is written down in Appendix A.4. Thematrix ${\hat{\mathit{H}}}_j$ contains several physical quantities such as the total mass, the center of total mass, the total inertia tensor, the total linear/angular momentum, etc. This feature of the matrix is also detailed in Appendix A.4.

4.4. Jacobians of joint torques

Let us compute matrix ${\hat{\mathit{N}}}_j$ which converts variation ${\delta }_{{\mathit{\chi }}_{\mathrm{all}}}$ of all joints to joint torque variation $\delta {\mathit{\tau }}_j$ of joint j as follows

$$\begin{array}{c}\delta {\mathit{\tau }}_j={\hat{\mathit{N}}}_j{\delta }_{{\mathit{\chi }}_{\mathrm{all}}}=\sum _k{\hat{\mathit{N}}}_{\left(j,k\right)}{\delta }_{{\mathit{\chi }}_k}\hfill \end{array}$$

(49)

where, ${\hat{\mathit{N}}}_{\left(j,k\right)}$ is the block matrix of ${\hat{\mathit{N}}}_j$ related to joint k.

The variance of the joint torque $\delta {\mathit{\tau }}_j$ can be obtained from Equation (21) as follows

$$\begin{array}{c}\delta {\mathit{\tau }}_j={\mathit{K}}_j^T\delta {\mathit{f}}_j^{p\left(j\right)}\hfill \end{array}$$

Therefore, ${\hat{\mathit{N}}}_{\left(j,k\right)}$ can be easily obtained by using Equation (40)

$$\begin{array}{c}{\hat{\mathit{N}}}_{\left(j,k\right)}={\mathit{K}}_j^T{\mathit{N}}_{(j,k)}\hfill \end{array}$$

(50)

4.5. An application: Jacobian of ZMP

Since ZMP is often used for the analysis of the balancing problem of humanoid systems, its Jacobian matrix will be useful in the motion optimization framework. This subsection presents the method of computing the Jacobian matrix of ZMP.

The total external forces acting on the kinematic chain are equivalent to ${\mathit{f}}_W^{p\left(W\right)}$ , where index W means the world coordinate. Note that the floating base-link $0$ is connected to the world coordinate via a 6-DOF free joint. By redefining $\mathit{F}\mathrm{ex}\triangleq {\mathit{f}}_W^{p\left(W\right)}$ , we can obtain ZMP projected, for example, on the x –y plane

$$\begin{array}{cc}{\mathit{p}}_{\mathrm{ZMP}}\hfill & \triangleq \left[\begin{array}{c}-F_{\mathrm{ex}\left(5\right)}∕F_{\mathrm{ex}\left(3\right)}\hfill \\ F_{\mathrm{ex}\left(4\right)}∕F_{\mathrm{ex}\left(3\right)}\hfill \\ 0\hfill \end{array}\right]\end{array}$$

(51)

where

$$\begin{array}{cc}{\mathit{F}}_{\mathrm{ex}}\hfill & \triangleq \left[\begin{array}{c}{F}_{\mathrm{ex}\left(1\right)}\hfill \\ ⋮\hfill \\ F_{\mathrm{ex}\left(6\right)}\hfill \end{array}\right]\end{array}$$

Let us compute matrix $\mathit{Z}$ which converts variation ${\delta }_{{\mathit{\chi }}_{\mathrm{all}}}$ of all joints to ZMP variation $\delta {\mathit{p}}_{\mathrm{ZMP}}$ as follows

$$\begin{array}{c}\delta {\mathit{p}}_{\mathrm{ZMP}}=\mathit{Z}{\delta }_{{\mathit{\chi }}_{\mathrm{all}}}=\sum _k{\mathit{Z}}_{\left(k\right)}{\delta }_{{\mathit{\chi }}_k}\hfill \end{array}$$

(52)

where, ${\mathit{Z}}_{\left(k\right)}$ is the block matrix of $\mathit{Z}$ related to joint k.

The variance of Equation (51) can be formulated as follows

$$\begin{array}{c}\delta {\mathit{p}}_{\mathrm{ZMP}}=\hat{\mathit{Z}}\delta {\mathit{F}}_{\mathrm{ex}}\left(=\mathit{Z}\delta {\mathit{f}}_W^{p\left(W\right)}\right)\hfill \end{array}$$

where

$$\begin{array}{c}\hat{\mathit{Z}}\triangleq \left[\begin{array}{cccccc}0\hfill & 0\hfill & \frac{F_{\mathrm{ex}\left(5\right)}}{{F_{\mathrm{ex}\left(3\right)}}^2}\hfill & 0\hfill & -\frac{1}{F_{\mathrm{ex}\left(3\right)}}\hfill & 0\hfill \\ 0\hfill & 0\hfill & -\frac{F_{\mathrm{ex}\left(4\right)}}{{F_{\mathrm{ex}\left(3\right)}}^2}\hfill & \frac{1}{F_{\mathrm{ex}\left(3\right)}}\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill \end{array}\right]\hfill \end{array}$$

Therefore, the Jacobian ${\mathit{Z}}_{\left(k\right)}$ can be computed as

$$\begin{array}{c}{\mathit{Z}}_{\left(k\right)}=\hat{\mathit{Z}}{\mathit{N}}_{\left(W,k\right)}\hfill \end{array}$$

(53)

5. Advanced implementation with decomposed gradient computation

Let us consider the following function

$$\begin{array}{c}h\left({\mathit{x}}_1,{\mathit{x}}_2,...,{\mathit{f}}_1,{\mathit{f}}_2,...,{\mathit{f}}_1^{p\left(1\right)},{\mathit{f}}_2^{p\left(2\right)},...\right)\hfill \end{array}$$

This function corresponds to the cost function or the each function of the corresponding constraint in Equation (1). The function contains several physical quantities such as link coordinates, link inertial forces, joint forces, etc. The gradient of the function with respect to each physical quantities is as follows

$$\begin{array}{c}\frac{\partial h}{\partial {\mathit{x}}_1},...,\frac{\partial h}{\partial {\mathit{f}}_1},...,\frac{\partial h}{\partial {\mathit{f}}_1^{p\left(1\right)}},...\hfill \end{array}$$

When solving the optimization problem such as Equation (1), the gradient of the function with respect to the joint variables $\mathit{\chi }\mathrm{all}$ is needed to compute Equation (2). By using the corresponding Jacobian matrices, the gradient can be computed as

$$\begin{array}{c}\frac{\partial h}{\partial \mathit{\chi }\mathrm{all}}=\frac{\partial h}{\partial {\mathit{x}}_1}{\mathit{J}}_1+...+\frac{\partial h}{\partial {\mathit{f}}_1}{\mathit{L}}_1+...+\frac{\partial h}{\partial {\mathit{f}}_1^{p\left(1\right)}}{\mathit{N}}_1+...\hfill \end{array}$$

(54)

In many cases, the cost function or each constraint function has a simple structure with respect to each physical quantity. A typical example for cost functions is the summation of the quadratic form of each physical quantity such as

$$\begin{array}{c}h=\frac{{\mathit{\omega }}_{J,1}}{2}\left|\right|{\mathit{x}}_1-{{\mathit{x}}_1}^{\mathrm{ref}}|{|}^2+...\hfill \\ +\frac{{\mathit{\omega }}_{L,1}}{2}\left|\right|{\mathit{f}}_1-{{\mathit{f}}_1}^{\mathrm{ref}}|{|}^2+...\\ +\frac{{\mathit{\omega }}_{N,1}}{2}\left|\right|{\mathit{f}}_1^{p\left(1\right)}-{\mathit{f}}_1^{p\left(1\right)}|{|}^2+...\end{array}$$

where, ${\mathit{\omega }}_{*,j}$ represents a weighing factor. In such a case, the gradients $\partial h∕\partial {\mathit{x}}_j$ , $\partial h∕\partial {\mathit{f}}_j$ , and $\partial h∕\partial {\mathit{f}}_j^{p\left(j\right)}$ for all j can be easily computed with computational complexity O $\left(N_j\right)$ . However, if ${\mathit{\omega }}_{*,j}\ne 0$ for all * and j, the computation of Equation (54) requires $N_j$ times of computation of the Jacobian matrices for each type of physical quantities. This leads the computational complexity of Equation (54) is O $\left(N_j^2\right)$ .

This section introduces an efficient computation method of Equation (54) with computational complexity O $\left(N_j\right)$ by avoiding the direct computation of the Jacobian matrices. The fast gradient computation is useful when solving a large-scale nonlinear optimization for a humanoid robot or a human, as mentioned in Ayusawa and Nakamura (2012). The computational cost of each iterative computation during optimization can be dramatically reduced by the combination of the fast gradient computation and the fast direction search algorithms such as the conjugate gradient method, the limited-memory quasi-Newton method, etc (Fletcher, 1987).

5.1. Gradient computation of arbitrary functions of ${\mathit{x}}_j$

Let us consider an arbitrary function $h$ whose variables are link coordinates ${\mathit{x}}_j$ for all j. This subsection introduces the computation method of the gradient $\partial h∕\partial {\mathit{\chi }}_{\mathrm{all}}$ , if the gradient $\partial h∕\partial {\mathit{x}}_j$ for all j is given.

The simple formulation when computing $\partial h∕\partial {\mathit{\chi }}_{\mathrm{all}}$ can be written by using Equation (31) as follows

$$\begin{array}{c}\frac{\partial h}{\partial {\mathit{\chi }}_{\mathrm{all}}}\left({\mathit{x}}_1,{\mathit{x}}_2,...\right)=\sum _j\frac{\partial h}{\partial {\mathit{x}}_j}\frac{\partial {\mathit{x}}_j}{\partial {\mathit{\chi }}_{\mathrm{all}}}=\sum _j\frac{\partial h}{\partial {\mathit{x}}_j}{\mathit{J}}_j\hfill \end{array}$$

(55)

This subsection introduces an efficient method of computing Equation (55).

The first step computes the gradient $\partial h∕\partial {\mathit{\xi }}_j^{p\left(j\right)}$ , which can be decomposed as follows

$$\begin{array}{c}\frac{\partial h}{\partial {\mathit{\xi }}_i^{p\left(i\right)}}=\sum _k\frac{\partial h}{\partial {\mathit{\xi }}_k}\frac{\partial {\mathit{\xi }}_k}{\partial {\mathit{\xi }}_i^{p\left(i\right)}}\hfill \end{array}$$

By using Equation (34), we can further have

$$\begin{array}{c}\frac{\partial h}{\partial {\mathit{\xi }}_i^{p\left(i\right)}}=\sum _k\frac{\partial h}{\partial {\mathit{\xi }}_k}{{\mathit{X}}_k^i}^{-1}{s}_{\left(k,i\right)}=\sum _{k\in \widehat{\mathcal{C}}\left(k\right)}\frac{\partial h}{\partial {\mathit{\xi }}_k}{{\mathit{X}}_k^i}^{-1}\hfill \end{array}$$

The above summation formulas can be transformed into the recursive formula as follows

$$\begin{array}{c}\frac{\partial h}{\partial {\mathit{\xi }}_i^{p\left(i\right)}}=\frac{\partial h}{\partial {\mathit{\xi }}_i}+\sum _{k\in C\left(i\right)}\frac{\partial h}{\partial {\mathit{\xi }}_k^{p\left(k\right)}}{{\mathit{X}}_k^{p\left(k\right)}}^{-1}\hfill \end{array}$$

(56)

Next, let us convert the variable $\mathit{\xi }$ into $\mathit{x}$ . The following relationship can be obtained according to Equation (12).

$$\begin{array}{c}\frac{\partial {\mathit{\xi }}_i^{p\left(i\right)}}{\partial {\mathit{x}}_i^{p\left(i\right)}}={\mathit{S}}_i^{p\left(i\right)},\frac{\partial {\mathit{\xi }}_i}{\partial {\mathit{x}}_i}={\mathit{S}}_i\hfill \end{array}$$

By using the above relationships, Equation (56) can be transformed into the following

$$\begin{array}{c}\frac{\partial h}{\partial {\mathit{x}}_i^{p\left(i\right)}}=\frac{\partial h}{\partial {\mathit{x}}_i}{\mathit{S}}_i^{-1}{\mathit{S}}_i^{p\left(i\right)}\hfill \\ +\sum _{k\in C\left(i\right)}\frac{\partial h}{\partial {\mathit{x}}_k^{p\left(k\right)}}{{\mathit{S}}_k^{p\left(k\right)}}^{-1}{{\mathit{X}}_k^{p\left(k\right)}}^{-1}{\mathit{S}}_k^{p\left(k\right)}\end{array}$$

Further, from Equation (33) we have

$$\begin{array}{c}\frac{\partial h}{\partial {\mathit{x}}_i^{p\left(i\right)}}=\frac{\partial h}{\partial \mathit{x}i}{\hat{\mathit{X}}}_i^i+\sum _{k\in C\left(i\right)}\frac{\partial h}{\partial {\mathit{x}}_k^{p\left(k\right)}}{\hat{\mathit{X}}}_k^{p\left(k\right)}\hfill \end{array}$$

(57)

Equation (57) indicates that, if the gradient $\partial h∕\partial \mathit{x}i$ for all i is given the gradient $\partial h∕\partial {\mathit{x}}_i^{p\left(i\right)}$ can be computed recursively from the end-links.

Finally, let us convert the variable ${\mathit{x}}_i^{p\left(i\right)}$ to ${\chi }_i$ . The following relationship can be obtained according to Equation (30) as follows

$$\begin{array}{c}\frac{\partial {\mathit{x}}_i^{p\left(i\right)}}{\partial {\mathit{\chi }}_i}={\mathit{G}}_i\hfill \end{array}$$

By using the above equation, Equation (57) can be transformed into

$$\begin{array}{c}\frac{\partial h}{\partial {\mathit{\chi }}_i}=\frac{\partial h}{\partial {\mathit{x}}_i^{p\left(i\right)}}{\mathit{G}}_i\hfill \end{array}$$

(58)

From the above formulations, the computation of Equation (55) can be archived by the following computations.

1. Compute Equation (57) recursively from the end-links.

2. Compute Equation (58).

The complexity of the decomposed gradient computation introduced in this subsection is $O\left(N_j\right)$ , whereas that of the direct computation is $O\left(N_j^2\right)$ .

5.2. Gradient computation of arbitrary functions of ${\mathit{f}}_j$

Let us consider an arbitrary function $h$ whose variables are link inertial forces ${\mathit{f}}_j$ for all j. This subsection introduces an efficient computation method of the gradient $\partial h∕\partial {\mathit{\chi }}_{\mathrm{all}}$ , if the gradient $\partial h∕\partial {\mathit{f}}_j$ for all j is given.

The simple formulation when computing $\partial h∕\partial {\mathit{\chi }}_{\mathrm{all}}$ can be written according to Equation (36) as follows

$$\begin{array}{c}\frac{\partial }{\partial {\mathit{\chi }}_{\mathrm{all}}}h\left({\mathit{f}}_1,{\mathit{f}}_2,...\right)=\sum _j\frac{\partial h}{\partial {\mathit{f}}_j}\frac{\partial {\mathit{f}}_j}{\partial {\mathit{\chi }}_{\mathrm{all}}}=\sum _j\frac{\partial h}{\partial {\mathit{f}}_j}{\mathit{L}}_j\hfill \end{array}$$

(59)

This subsection presents a method of computing Equation (59).

First, let us consider the gradient $\partial h∕\partial {\mathit{x}}_j$ . It can be decomposed as follows

$$\begin{array}{c}\frac{\partial h}{\partial {\mathit{x}}_j}=\sum _k\frac{\partial h}{\partial {\mathit{f}}_k}\frac{\partial {\mathit{f}}_k}{\partial {\mathit{x}}_j}=\frac{\partial h}{\partial {\mathit{f}}_j}\frac{\partial {\mathit{f}}_j}{\partial {\mathit{x}}_j}\hfill \end{array}$$

From Equation (38), we can have

$$\begin{array}{c}\frac{\partial {\mathit{f}}_j}{\partial {\mathit{x}}_j}={\mathit{H}}_j\hfill \end{array}$$

Finally, the above equations can be transformed into

$$\begin{array}{c}\frac{\partial h}{\partial {\mathit{x}}_j}=\frac{\partial h}{\partial {\mathit{f}}_j}{\mathit{H}}_j\hfill \end{array}$$

(60)

Since Equation (60) provides $\partial h∕\partial {\mathit{x}}_j$ , the gradient $\partial h∕\partial {\mathit{\chi }}_{\mathrm{all}}$ can be computed in the same manner when computing Equation (55) in the previous section.

5.3. Gradient computation of arbitrary functions of ${\mathit{f}}_j^{p\left(j\right)}$

Let us consider an arbitrary function $h$ whose variables are joint constraint forces ${\mathit{f}}_j^{p\left(j\right)}$ for all j. This subsection introduces an efficient method to compute the gradient $\partial h∕\partial {\mathit{\chi }}_{\mathrm{all}}$ , if the gradient $\partial h∕\partial {\mathit{f}}_j^{p\left(j\right)}$ for all j is given.

The simple formulation when computing $\partial h∕\partial {\mathit{\chi }}_{\mathrm{all}}$ can be written by using Equation (40) as follows

$$\begin{array}{c}\frac{\partial }{\partial {\mathit{\chi }}_{\mathrm{all}}}h\left({\mathit{f}}_1^{p\left(1\right)},...\right)=\sum _j\frac{\partial h}{\partial {\mathit{f}}_j^{p\left(j\right)}}\frac{\partial {\mathit{f}}_j^{p\left(j\right)}}{\partial {\mathit{\chi }}_{\mathrm{all}}}=\sum _j\frac{\partial h}{\partial {\mathit{f}}_j^{p\left(j\right)}}{\mathit{N}}_j\hfill \end{array}$$

(61)

This subsection introduces its efficient computation.

First, let us formulate the variation of ${\mathit{f}}_j^{p\left(j\right)}$ by using Equation (20) as follows

$$\begin{array}{c}\delta {\mathit{f}}_j^{p\left(j\right)}=\sum _{k\in \widehat{\mathcal{C}}\left(j\right)}\left({{\mathit{A}}_k^j}^{-T}\delta {\mathit{f}}_k+\delta \left({{\mathit{A}}_k^j}^{-T}\right){\mathit{f}}_k\right)\hfill \end{array}$$

(62)

The matrix variation $\delta \left({{\mathit{A}}_k^j}^{-1}\right)$ can be obtained from the relationship ${\mathit{A}}_k^j{{\mathit{A}}_k^j}^{-1}=\mathit{E}$ , and we can have

$$\begin{array}{c}\delta \left({{\mathit{A}}_k^j}^{-1}\right)=-{{\mathit{A}}_k^j}^{-1}\left(\delta {\mathit{A}}_k^j\right){{\mathit{A}}_k^j}^{-1}=-{{\mathit{A}}_k^j}^{-1}\left({\mathit{A}}_k^j\left[\delta {\mathit{\alpha }}_k^j\bullet \right]\right){{\mathit{A}}_k^j}^{-1}\hfill \\ =-\left[\delta {\mathit{\alpha }}_k^j\bullet \right]{{\mathit{A}}_k^j}^{-1}\end{array}$$

Therefore, the term $\delta \left({{\mathit{A}}_k^j}^{-T}\right){\mathit{f}}_k$ can be transformed as follows

$$\begin{array}{c}\delta \left({{\mathit{A}}_k^j}^{-T}\right){\mathit{f}}_k=-{{\mathit{A}}_k^j}^{-T}{\left[\delta {\mathit{\alpha }}_k^j\bullet \right]}^T{\mathit{f}}_k=-{{\mathit{A}}_k^j}^{-T}\left[{\mathit{f}}_k\hat{\bullet }\right]\delta {\mathit{\alpha }}_k^j\hfill \end{array}$$

By substituting $\delta {\mathit{\alpha }}_k^j=\delta {\mathit{\alpha }}_k-{{\mathit{A}}_k^j}^{-1}\delta {\mathit{\alpha }}_j$ into the above equation, we have

$$\begin{array}{c}\delta \left({{\mathit{A}}_k^j}^{-T}\right){\mathit{f}}_k=-{{\mathit{A}}_k^j}^{-T}\left[{\mathit{f}}_k\hat{\bullet }\right]\delta {\mathit{\alpha }}_k+{{\mathit{A}}_k^j}^{-T}\left[{\mathit{f}}_k\hat{\bullet }\right]{{\mathit{A}}_k^j}^{-1}\delta {\mathit{\alpha }}_j\hfill \\ =-{{\mathit{A}}_k^j}^{-T}\left[{\mathit{f}}_k\hat{\bullet }\right]\delta {\mathit{\alpha }}_k+\left[({{\mathit{A}}_k^j}^{-T}{\mathit{f}}_k{)}_{\hat{\bullet }}\right]\delta {\mathit{\alpha }}_j\end{array}$$

By using this relationship, Equation (62) can be transformed into

$$\begin{array}{c}\delta {\mathit{f}}_j^{p\left(j\right)}=\sum _{k\in \widehat{\mathcal{C}}\left(j\right)}\left\{{{\mathit{A}}_k^j}^{-T}\left(\delta {\mathit{f}}_k-\left[{\mathit{f}}_k\hat{\bullet }\right]\delta {\mathit{\alpha }}_k\right)\right\}\hfill \\ +\left[\left(\sum _{k\in \widehat{\mathcal{C}}\left(j\right)}{{\mathit{A}}_k^j}^{-1}{\mathit{f}}_k\right)\hat{\bullet }\right]\delta {\mathit{\alpha }}_j\end{array}$$

By substituting Equation (40) and $\delta {\mathit{\alpha }}_{*}=\mathit{T}\delta {\mathit{x}}_{*}$ into the above, we have

$$\begin{array}{c}\delta {\mathit{f}}_j^{p\left(j\right)}=\sum _{k\in \widehat{\mathcal{C}}\left(j\right)}\left\{{{\mathit{A}}_k^j}^{-T}\left(\delta {\mathit{f}}_k-\left[{{\mathit{f}}_k}_{\hat{\bullet }}\right]\mathit{T}\delta {\mathit{x}}_k\right)\right\}+\left[{{\mathit{f}}_j}_{\hat{\bullet }}\right]\mathit{T}\delta {\mathit{x}}_j\hfill \end{array}$$

Then, the following partial derivative holds

$$\begin{array}{c}\frac{\partial {\mathit{f}}_j^{p\left(j\right)}}{\partial {\mathit{x}}_k}=s_{(j,k)}{{\mathit{A}}_k^j}^{-T}\left({\mathit{H}}_k-\left[{{\mathit{f}}_k}_{\hat{\bullet }}\right]\mathit{T}\right)+{\delta }_{j,k}\left[{{\mathit{f}}_j^{p\left(j\right)}}_{\hat{\bullet }}\right]\mathit{T}\end{array}$$

(63)

where, ${\delta }_{j,k}$ is the Kronecker delta.

Next, let us compute the gradient $\partial h∕\partial {\mathit{x}}_k$ . It can be decomposed as follows

$$\begin{array}{cc}\frac{\partial h}{\partial {\mathit{x}}_k}\hfill & =\sum _j\frac{\partial h}{\partial {\mathit{f}}_j^{p\left(j\right)}}\frac{\partial {\mathit{f}}_j^{p\left(j\right)}}{\partial {\mathit{x}}_k}\end{array}$$

(64)

By substituting Equation (63) into the above, the gradient can be computed as follows

$$\begin{array}{cc}\frac{\partial h}{\partial {\mathit{x}}_k}\hfill & ={d_k}^T\left({\mathit{H}}_k-\left[{{\mathit{f}}_k}_{\hat{\bullet }}\right]\mathit{T}\right)+\frac{\partial h}{\partial {\mathit{f}}_j^{p\left(j\right)}}\left[{{\mathit{f}}_j^{p\left(j\right)}}_{\hat{\bullet }}\right]\mathit{T}\end{array}$$

(65)

where

$$\begin{array}{c}{d}_k\triangleq \sum _{j\in \widehat{\mathcal{C}}\left(k\right)}{{\mathit{A}}_k^j}^{-1}{\left(\frac{\partial h}{\partial {\mathit{f}}_k^{p\left(k\right)}}\right)}^T\hfill \end{array}$$

The vector $d_k$ can be computed from the following recursive form

$$\begin{array}{c}{d}_k={\left(\frac{\partial h}{\partial {\mathit{f}}_k^{p\left(k\right)}}\right)}^T+\sum _{j\in C\left(k\right)}{{\mathit{A}}_k^j}^{-1}{d}_j\end{array}$$

(66)

Finally, the gradient $\partial h∕\partial {\mathit{x}}_k$ can be computed from the following steps.

1. Compute Equation (66) for all k from the end-links.

2. Compute Equation (65) for all k.

Since $\partial h∕\partial {\mathit{x}}_k$ for all k is provided, the gradient $\partial h∕\partial {\mathit{\chi }}_{\mathrm{all}}$ can be computed in the same manner when computing Equation (55).

5.4. Gradient computation of other functions

The joint torque ${\mathit{\tau }}_j$ can be represented by ${\mathit{f}}_j^{p\left(j\right)}$ according to Equation (21). Therefore, the gradient of an arbitrary function $h$ of the joint torque ${\mathit{\tau }}_j$ with respect to ${\mathit{f}}_j^{p\left(j\right)}$ can be decomposed into

$$\begin{array}{c}\frac{\partial h}{\partial {\mathit{f}}_j^{p\left(j\right)}}=\frac{\partial h}{\partial {\mathit{\tau }}_j^{p\left(j\right)}}\frac{\partial {\mathit{\tau }}_j}{\partial {\mathit{f}}_j^{p\left(j\right)}}=\frac{\partial h}{\partial {\mathit{\tau }}_j^{p\left(j\right)}}{\mathit{K}}_j^T\end{array}$$

(67)

When the function contains the joint torque ${\mathit{\tau }}_j$ , the gradient can be computed from Equation (61) and Equation (67).

Similar to the case of the joint torque, ZMP ${\mathit{p}}_{\mathrm{ZMP}}$ can be represented by ${\mathit{f}}_W^{p\left(W\right)}$ according to Equation (51). The gradient of an arbitrary function $h$ of ${\mathit{p}}_{\mathrm{ZMP}}$ with respect to ${\mathit{f}}_j^{p\left(j\right)}$ can be decomposed into

$$\begin{array}{c}\frac{\partial h}{\partial {\mathit{f}}_W^{p\left(W\right)}}=\frac{\partial h}{\partial {\mathit{p}}_{\mathrm{ZMP}}}\frac{\partial {\mathit{p}}_{\mathrm{ZMP}}}{\partial {\mathit{f}}_W^{p\left(W\right)}}=\frac{\partial h}{\partial {\mathit{p}}_{\mathrm{ZMP}}}\hat{\mathit{Z}}\end{array}$$

(68)

When the function contains ${\mathit{p}}_{\mathrm{ZMP}}$ , the gradient can be computed from Equation (61) and Equation (68).

6. Numerical evaluation

6.1. Comparison of computation time of Jacobian matrices

We here show the comparison of the computation times of the Jacobian matrices for the three approaches: the proposed method shown in Section 4, the traditional method in Suleiman et al. (2008), and the automatic differentiation method. Since the formulations in Suleiman et al. (2008) only handle 1-DOF joints, it was tested by using a serial manipulator with N rotational joints. The Jacobian matrix of the joint torque of the first rotational joint was computed by changing the number of joints. We implemented the automatic differentiation of the joint torque computation by using Adept (Hogan, 2014): a combined automatic differentiation and array library for C++. The methods were tested on the computer with Intel(R) Xeon(R) CPU E3-1535M v5. The proposed method generated the same Jacobian matrices as those from the other two methods. Figure 3 shows the results of the computational time of the two methods, demonstrating its correctness.

Fig. 3.

Comparison of computation time of joint torque Jacobian matrix.

The computational complexity of the conventional method was $O\left(N^2\right)$ and that of the proposed method was O(N). The computational time was significantly improved in the large-DOF cases. Though the computational complexity of the automatic differentiation method is also O(N), the computational time of each DOF case was about 5 times higher than that of the proposed method. Since the proposed method is based on the efficient recursive formula, it shows better computational performance than the automatic differentiation method.

6.2. Comparison of computation time of gradient

This subsection evaluates the performance of the proposed method of computing the gradient of the cost function. We tested the method by using the same serial manipulator model used in the previous subsection. The following two cases were considered.

• The cost function contains the joint torque of only one joint: $c=\left|\right|\mathit{\tau }1|{|}^2$

• The cost function contains the joint torque for all joints: $c={\sum }_j\left|\right|{\mathit{\tau }}_j|{|}^2$

We computed $\partial c∕\partial {\mathit{\chi }}_{\mathrm{all}}$ for the two cases by the two approaches: the proposed decomposed gradient computation (DGC) shown in Section 5 and the normal approach according to Equation (54) without DGC. In the normal approach, the Jacobian matrices were computed by the method shown in Section 4.

The comparison of computation times in case (A) is shown in Figure 4, and that in case (B) is in Figure 5. The solid lines represent the computation time with DGC, and the dotted lines indicate the time without DGC. The gradient computation without DGC in case (A) requires the Jacobian matrix of the joint torque of only one joint. This leads that the computational complexity without DGC is also O( $N_J$ ), which can be seen from the dotted line in Figure 4. The improvement of computational speed by DGC is not significant for small number of evaluated quantities. However, the gradient computation without DGC in case (B) needs the Jacobians for all the joints, and the computational complexity becomes O( ${N_J}^2$ ), as shown in Figure 5. In the both cases, the computational complexity was O( $N_J$ ) when computing the gradient with DGC. When the number of evaluated quantities in the cost function increases, the proposed method significantly reduced the computational time of the gradient computation.

Fig. 4.

Comparison of computation time for gradient computation when the number of constraint is 1. The solid line (with DGC) represents the time with the proposed decomposed gradient computation. The dotted line (without DGC) indicates the time without the decomposed gradient computation and with computing the Jacobian matrices.

Fig. 5.

Comparison of computation time for gradient computation when the number of constraint is $N_j$ .

6.3. Motion optimization of spherical joint manipulator

The proposed method is applied to a redundant serial robot manipulator composed of five spherical joints, in order to validate the Jacobian for the case of spherical joints. The 15-DOF manipulator moved in a complex environment cluttered with non-convex obstacles and no gravity. Each link featured the following physical quantities: for link i, $\mathit{p}p\left(i\right)i={\left[000.2\right]}^{\mathrm{T}}\mathrm{m}$ , $\mathrm{mi}=3.0\mathrm{kg}$ , $\mathit{c}i={\left[000.1\right]}^{\mathrm{T}}\mathrm{kg\; m}$ , $\mathit{I}i=\mathrm{diag}\left(\left[11.211.22.4\right]\right)*10^{-3}{\mathrm{kg\; m}}^2$ . Let $p_e^{\left(t\right)}$ be the position of the end-effector at time instance t, and ${\mathit{\tau }}_{j,k}\left(1\le j\le 5,1\le k\le 3\right)$ be k th component of torques of j -th spherical joint.

In this trajectory optimization, the total time length of the trajectory was assumed to be 2 s, and its sampling rate was 5 ms. The variables of the spherical joints were represented by an angle-axis vector, and their trajectories were modeled using B-splines, as shown in Suleiman et al. (2008). The number of B-spline bases was 24, and the timespan between the two bases was 0.1 s. The position of the end-effector started from $p_e^0={\left[001\right]}^{\mathrm{T}}$ with zero joint velocities and accelerations, passed through $p_e^1={\left[010\right]}^{\mathrm{T}}$ at $t=1.0$ s, and stopped at $p_e^2={\left[100\right]}^{\mathrm{T}}$ with zero joint velocities and accelerations. During the movement, joint torque limitations were considered; $\left|{\mathit{\tau }}_{1,k}\right|<0.1$ N and $\left|{\mathit{\tau }}_{j,k}\right|<10.0$ N $\left(2\le j\le 5\right)$ . This means that the manipulator is imposed with a severe constraint, that it should be controlled with almost zero torque generated at the root joint. To avoid collisions and self-collision, several bounding primitives are located to cover the manipulator and the obstacles. We created the bounding sphere $B{J}_j$ on each joint j and the bounding capsule $B{L}_l$ on each link l. Each obstacle k is covered by the set of four bounding capsules $B{O}_k$ . The radius of each primitive is as follows: 0.055 m ( $B{J}_j$ ), 0.035 m ( $B{L}_l$ ), and 0.05 m ( $B{O}_k$ ). We assumed the constraints on the distance from $B{O}_k$ to $B{J}_i$ and $B{L}_j$ for collision avoidance, and those on the distance among $B{J}_j$ and $B{L}_l$ for self-collision avoidance. To obtain the smooth joint torque trajectories, the cost function of energy consumption is also considered. It is formulated by the sum of the squared energy consumption of each joint: ${\sum }_{j,k}|{\mathit{\tau }}_{j,k}{\mathit{\psi }}_{j,k}{|}^2$ .

All the constraint conditions were converted to penalty cost functions according to the penalty function method

$$\begin{array}{c}\underset{\mathit{a}}{min}c\left(\mathit{Y}\right)+\frac{1}{2}\sum _k{W}_k{\left(max\left(g_k\left(\mathit{Y}\right),0\right)\right)}^2\hfill \end{array}$$

(69)

where, $max\left(\mathit{a},b\right)$ returns the larger of two numbers, and $W_k$ represents a penalty weight for the k th constraint. It should be noted that the problem can be solved by other methods like sequential quadratic programing. Though the penalty function method allows the slight violation of the constraints, it enables the fast computation when it is used in conjunction with DGC, as mentioned in Ayusawa and Nakamura (2012) The optimization itself was solved by the quasi-Newton method with line search. The gradient of the cost function was computed with and without DGC, respectively, to compare their computational speed. The initial value of the B-spline trajectory for the optimization was such that all the angle-axis vectors for the spherical joint are equal to zero for all time instances. The total computation time of the motion optimization without DGC was 631.9 s. DGC accelerated the optimization so that its computation time was 233.5 s.

The generated trajectory is shown in Figure 6. The end-effector successfully passed through the targeted positions without colliding with any obstacles. The results of the joint torque trajectories for the first joint are shown in Figure 7, and they all satisfy the joint torque limitations. The other joint torques also satisfied the limitations as shown in Figure 8.

Fig. 6.

Snapshots of the generated motion of the redundant robot in a cluttered environment. Starting from the initial position ${\left[001\right]}^{\mathrm{T}}$ , the target end-effector positions at ${\left[010\right]}^{\mathrm{T}}$ ( $t=1.0$ s) and ${\left[100\right]}^{\mathrm{T}}$ ( $t=2.0$ s) were achieved without collision to obstacles.

Fig. 7.

Resultant joint torque trajectories of the base spherical joint. The torque limit of small value of $\pm 0.1$ Nm is satisfied throughout the motion.

Fig. 8.

Resultant joint torque trajectories of all the joints except for the base joint. All the 12 lines are within the torque limit of $\pm 10$ Nm throughout the motion.

6.4. Motion optimization of humanoid robot

As one of the practical applications where the proposed framework is useful, this subsection shows an example of the motion optimization for dynamic parameter identification of a humanoid robot (Ayusawa et al., 2017).

The optimal trajectory for dynamic parameter identification is called the persistent exciting (PE) trajectory (Gautier and Khalil, 1992). To generate the PE trajectory, the condition number of the “regressor matrix” obtained from the joint trajectory should be minimized while maintaining dynamic constraints. Although we showed an analytical framework to optimize the condition number in our previous work (Ayusawa et al., 2017), the stability was considered only statically by using the center of mass (CoM), which is conservative and may limit the identification performance. In the resultant motions, both feet were placed to the ground because the dynamic stability condition became too severe to be satisfied only by the CoM condition. This limitation makes it difficult to generate dynamic leg motions by standing on one leg for better identification. In the proposed framework, the analytical computation of the Jacobian of the ZMP can be provided, which can guarantee the dynamic stability constraint in the trajectory optimization.

We have derived a dynamically stable optimal PE trajectory on one leg by constraining the ZMP inside the area of 4 cm and 1 cm around the center of the standing foot in front and lateral direction. The trajectory is parameterized by B-Spline using physical properties from the robot CAD model. We also added constraints on forces and torque applied on the ankle so that the horizontal forces $\mathrm{Fex}\left(1\right),\mathrm{Fex}\left(2\right)$ and torque $\mathrm{Fex}\left(6\right)$ stay within $\pm 20$ N, $\pm 20$ N, $\pm 4$ Nm respectively to avoid slipping. To prevent the jumping of the robot, we also set the limitation on the vertical force $\mathrm{Fex}\left(2\right)$ such that $|\mathrm{Fex}\left(2\right)-M_{r}g|\le 50$ N, where $M_r$ is the total mass of the robot. Similar to the example in the previous section, all equality and inequality constraints were treated as penalty functions by the penalty-function method. Then, the computation was performed by the proposed decomposed gradient computation.

As shown in the snapshots in Figure 9, the humanoid HRP-4 (Kaneko et al., 2011) successfully performed the optimized PE trajectory on dynamic simulator Choreonoid (Nakaoka, 2012), which validates the feasibility. The total computation time without DGC was 12826 s. DGC accelerated the optimization so that its computation time was 2857 s. The result of the optimized condition number was 124.2, small enough to indicate that a dynamically stable optimal trajectory for dynamics identification of the robot were generated successfully by using the proposed framework.

Fig. 9.

Snapshots of optimized one-leg PE trajectory for dynamic parameter identification. The humanoid robot HRP-4 can successfully perform resultant whole-body motion by also exciting the free leg.

7. Conclusion

This paper presented a comprehensive theory of differential kinematics and dynamics to derive analytical partial derivatives of both link/joint quantities with respect to joint coordinates and their derivatives. First, the 18×18 comprehensive motion transformation matrix (CMTM) and 18-dimensional product operation was introduced for comprehensive kinematics formulation, which allows a simple chain product of the matrices to represent the differential forward kinematics of a kinematics chain. They also have the same features as the rotation matrix and the 6×6 transformation matrix, and their product operations. By utilizing CMTM, the partial derivative of the link coordinates and their derivatives were derived in the same manner as when introducing the basic Jacobians by just replacing the 6×6 transformation matrices with CMTM in their formulations.

This novel theoretical framework added the following contributions to current work. The partial derivatives of each generalized force with respect to the joint coordinates and their derivatives were also demonstrated. The derivation procedure also has a similarity to that of the linear and angular momentum Jacobians. The formulation can also handle different types of joints such as spherical joints or free-floating bases. We have also shown that the analytical derivative of physical quantities like ZMP can be easily computed with the proposed method, which is an important advantage for practical human/humanoid motion optimization. By utilizing the CMTM, each new Jacobian could be computed with O( $N_J$ ), which was verified by the comparison of computational times from the proposed and conventional methods. The proposed method was also compared to the automatic differentiation and showed better computational performance thanks to the efficient recursive formula of the method.

The optimization problem often can be solved efficiently by avoiding the direct computation of Jacobian matrices, as indicated in Ayusawa and Nakamura (2012). The fast gradient computation algorithms were also proposed to lead to more practical implementation. Evaluation of cost function composed of different types of physical quantities usually requires a heavy computational cost. Together with the decomposed gradient computation method (Ayusawa and Nakamura, 2012), the gradient computation could be performed with computational complexity O( $N_J$ ).

A couple of application examples were presented to demonstrate the usefulness of the proposed framework. We showed the dynamic trajectory optimization of a redundant serial robot manipulator composed of spherical joints. A collision-free dynamic motion was successfully generated in a cluttered environment with non-convex obstacles, while simultaneously imposing a strong torque limit. This validates the basic trajectory optimization capacity of the proposed framework under severe constraints. Another application is optimization of the PE trajectory for identification of dynamic parameters of a humanoid robot. In this example, the analytical gradient of ZMP with respect to joint angle and its derivatives was utilized to guarantee the balance. Dynamic one-leg PE motions were generated and their validity was confirmed via a dynamic simulator.

The mathematical features of CMTM and its operator have a similarity to those of the rotational matrix and the cross product or those of the spatial transformation matrix and the screw operation. The rotational or screw motion of a rigid body has been studied from the view point of a Lie group and Lie algebra. Future work will focus on the features of CMTM from that point of view.

This paper represented the trajectory variables in the motion optimization of the joint coordinates and introduced the Jacobian of forces with respect to the joint coordinates according to the inverse dynamics formula. Concerning control issues, we often consider the joint torques as controller input variables and optimize them. Though the Jacobian with respect to the joint torques can be obtained by computing the inverse matrices, there is another possibility of inverse Jacobian formulation according to the efficient formula used in forward dynamics computation (Featherstone, 1983), which will be addressed in our future work.

There is still room for improvement in computation time of the proposed method by using parallel computation for multi-body systems (Featherstone, 2008). An algorithm for parallel computation will be investigated for future applications such as the real-time control of a humanoid robots or the motion analysis for a complicated human skeletal system.