Energy-Shaping Controllers for Soft Robot Manipulators Through Port-Hamiltonian Cosserat Models

In this work, we discuss the application of energy-based controller design for under-actuated soft robot manipulators. The continuous dynamics of the soft robot are modeled through the differential geometry of Cosserat beams. Using a finite-dimensional truncation, the system can be written as a reduced port-Hamiltonian model that preserves the passivity condition. Then, a model-based controller is introduced that produces a local minimizer of closed-loop potential energy for the desired end-effector configuration. The stabilizing control utilizes an energy-based approach and exploits the passivity of the soft robotic system. The effectiveness of the energy-based controller is demonstrated through extensive simulations of various soft robotic systems that share a resemblance with biology. All software and numerical studies are provided in an open-access SOROTOKI toolkit written in Matlab.


Introduction
The field of soft robotics is slowly growing as a prominent successor to conventional rigid robotics. Contrary to rigid robots, soft robots explore 'soft materials' that significantly enhance the robot's dexterity, inherent safety, enable a rich family of motion primitives, and provide environmental robustness. By fully exploiting soft materials, soft robotics places the first steps towards achieving performance similar to biology [12,16,21]. In this work, we primarily focus on a subclass of soft robots called 'soft manipulators'.
Although significant steps have been taken towards bridging biology and soft robotics, its innate infinite-dimensionality poses substantial challenges to modeling and control. To be more specific, soft robots theoretical allow for infinitely many degrees-of-freedom along their continuously deformable body. This renders them particularly suited for PDE models [15,20,33] rather than the conventional ODE for traditional robotics [22,31]. Additionally, their actuation often employs distributed loads (e.g., pneumatics [16,21] and tendons [32,33]). Consequently, classical descriptions of rigid links and joints paired with local actuation are no longer viable nor physically representative. This paradigm shift calls for novel control-oriented modeling approaches tailored for hyper-flexible and underactuated robotic systems.
In the last decade, the field of modeling for soft robotic systems has matured sufficiently and currently their applicability in model-based control is slowly feasible [14]. To highlight a few: reduced-order finite element models [15,33,34], constant and non-constant curvature approaches [13,18], Cosserat-beam models [4,27], and learningbased approaches [5]. The Piece-wise Constant Curvature (PCC) model-a popular method of state reduction that assumes piecewise constant strains along the soft robot's body-has proven to be viable for modeling solution applicable to feedforward controllers [16], and more recently model-based feedback controllers [13,18]. Nevertheless, the PCC approach has limitations. They do not originate

Dynamic Modeling
Throughout this work, we will explore Lie group theory and various notations that we briefly wish to introduce. We provided a nomenclature in Table 1. The following notations are adopted: the Lie group of rigid-body transformation Space curve (i.e., Cosserat backbone) Geometric strain field on se(3) Geometric velocity field on se(3) q(t) Modal coefficients or joint variables ( ) Modal basis or shape function  (3) , respectively. The mathematical operations related to the groups and their algebras, e.g., the adjoint actions, are also provided in the nomenclature 1.

Cosserat Beam Theory
In Cosserat theory, slender deformable solids are modeled as elastic strings subjected to geometric finite-strain theory.
Drawing an analogy with soft robotics, we model the soft robot as a one-dimensional spatial curve passing through the geometric center of the soft robot (see Fig. 1). A similar approach is followed in our previous work [9]. Given its spatial-temporal nature, we introduce a temporal variable t ∈ [0, T] with finite horizon time T, and a spatial variable ∈ [0, L] with L the undeformed length of the soft robot. For convenience, we denote = [0, T] and = [0, L] . For each point on the backbone, we introduce a (mobile) coordinate frame. The homogeneous rotation related to these coordinate frames is given by the rotation matrix ∶ × → SO(3) , and their origin by the position vector ∶ × → ℝ 3 . Following the geometric approach [3,4,26,27,29], we may equivalently represent each coordinate frames that are rigidly attached to the continuous backbone of the soft robot by a parameterized space curve in SE(3): Now, an expression for the strain field and velocity field anywhere on the Cosserat beam can be found by exploring the differential geometry of the curve. To do so, we must introduce some smoothness criteria: Assumption 1 All control inputs, i.e., a distributed control wrench acting on the system, are considered to be sufficiently smooth for any instance t ∈ and ∈ such that the resulting parametrized backbone g( , t) ∈ SE(3) is everywhere differentiable.

Local Strain and Velocity
Following the works [4,9,26,27], let Γ = ( 1 , 2 , 3 ) ⊤ and U = ( 1 , 2 , 3 ) ⊤ be the torsion-curvature and elongationshear strain vector, respectively. Then, an expression for strain field ( , t) is obtained through spatial differentiation of g: Similarly, let Ω = ( 1 , 2 , 3 ) ⊤ and V = (v 1 , v 2 , v 3 ) ⊤ be the angular and linear velocity vector, respectively. Then, an expression for velocity field ( , t) is obtained through time differentiation of g: Since we assume g to be everywhere differentiable, we can derive a PDE for the continuous forward kinematics of the soft robot [4,9,27]: where ad (⋅) denotes the adjoint action on the Lie algebra (full derivation in Appendix 1). Drawing an analogy to rigid robotics, the expression in (4) may be seen as the forward kinematics for a serial chain manipulator with infinitely many links.

Finite-Dimensional Reduction
Similar to finite element methods, we wish to find a finitedimensional approximation of the strain field ( , t) for all points on the material domain . To do so, we assume the following [9]: Assumption 2 Assuming the strain field has a separable spatio-temporal nature, any entry of the strain vector field = 1 , 2 , … , 6 ⊤ can be written as an infinite expansion of the following form: where { n } ∞ n=1 is the set of (orthogonal) basis functions n ∶ → ℝ together with modal coefficients q i,n ∶ → ℝ , and an intrinsic time-invariant strain • i ∶ → ℝ . The basis functions n (⋅) and the modal coefficients q n (⋅) are both smooth functions. Assumption 3 Given infinite expansion (5), the k-th order truncation for any entry of the strain field, defined as converges uniformly on and as the index k → ∞ . Moreover, we assume that uniform convergence holds for its partial derivatives t [ ] k and [ ] k .
(4) = −ad +̇, Accordingly, we can rewrite the k-th order truncation of the complete strain field as a linear matrix operation as follows where ∈ ℝ 6×6k is a sparse matrix-valued function whose columns are mutually orthonormal, the operator ⊗ denotes the Kronecker product and the vector q ∈ ℝ 6k the collection of all time-variant modal coefficients related to the columns of . Although a wide variety of bases are possible [4,13], we have chosen a modified Legendre polynomial set: with n ∈ ℤ + the polynomial degree. Please note now that the inner product between elements of the set of modified Legendre functionals { n } k n=1 satisfies ⟨ i , j ⟩ ∶= ∫ i j d = 0 for i ≠ j , and 1 otherwise. An alternative option could be constructing the set of basis functions through the so-called 'snapshot decomposition method' using FEM-driven data [1,15,20].

Finite-Dimensional Kinematics
Given the finite-dimensional truncation in (7), we can now find an expression for the finite-dimensional forward kinematics in terms of the generalized coordinates q and its velocities components ̇q.
First, let us regard the configuration of the soft robot g ∈ SE(3) . Recall that the spatial evolution of the curve is described by g∕ = g ∧ , see Eq. (2). Given the initial condition g(0, ⋅) = g 0 , an approximation of the continuously deformable soft robot can be obtained by partial integration over the spatial domain: Please note that this nothing more than the reconstruction of the curve by integration of its tangent space along its spatial parameter . Next, lets regard the velocity kinematics ( , t) for any point on the backbone curve. Using the differential property of the adjoint action ad = − ∕ [Ad g −1 ]Ad g [22], we can rewrite the continuous forward kinematics in (4) as Now, given the initial condition (0, t) = 0 6 and the approximations [ ] k and [g] k , we can find an approximation to the velocity twist by partial integration over space: which naturally gives rise to the geometric Jacobian [J] k ∈ ℝ 6×6k . The geometric Jacobian plays an important role in obtaining the Lagrangian form of the reduced-order dynamic model. Finally, to express the acceleration twist, we take the time-derivative of (11) leading to which gives rise to the analytic expression of the time-derivative of the geometric Jacobian [ J] k (see Appendix 2 for the derivation).

Finite-Dimensional Dynamics
Here, we detail the dynamics of the Cosserat beam. A majority of the dynamic framework presented here is adopted from [4]; yet we introduce some modification to allow for the pHstructure. Earlier work of this extension can also be found in [9], but recapitulated here for to be self-contained. First, let us consider an infinitesimal slice of continuum body that is perpendicular to the backbone curve. The kinetic momenta of this infinitesimal slice are then given by

Remark 1
For some soft robots, the inertia tensor M may have an explicit dependency on space or time (or both). Nevertheless, for sake of simplicity, we limit ourselves to a diagonal invariant inertia tensor M = diag I 3 , J with line-density > 0 and the second moment of area Using the expression of the kinetic momenta of the infinitesimal body, we can write the equation of motion for a particular slice at using the Newton-Euler equations: where again Ad (⋅) stands for the adjoint action, and F = F c + F u + F g − F e the resultant wrench that is composed of the constraint wrench F c , the input wrench F u , and the potential wrenches due to gravity and visco-elasticity, F g and F e , respectively. Further evaluation of (13) leads to (11) Before continuing, we introduce a slight modification to the relation above. Using the fact that ad = 0 6 , we can introduce the vector Mad to (14) without affecting the dynamics. The importance of this modification originates from the preservation of passivity in the Lagrangian model, which is an important property in stability theorems for robotics. By substitution of the null vector, the equation of motion becomes which is nothing more than the Newton-Euler equation for rigid-body motion on ℝ 3 . To introduce the (reduced-order) Cosserat kinematics and make the expression symmetric, we substitute (11) and (12) into (15) and pre-multiply by It is important to note that by pre-multiplication of the transpose Jacobian, we have eliminated the constraint wrenches, i.e., [22]. Finally, the finite-dimensional dynamics of the deformable soft robot is found by spatial integration of (16) over the material domain . The overall dynamics can be written in familiar Euler-Lagrangian (EL) form as follows with the system matrices: where M is the generalized inertia matrix, C the centripetal-Coriolis matrix, N a vector of generalized potential forces with F g = −Ad ⊤ Ma g the external wrench acting on the body due to gravitational acceleration a g , and F e a vector of viscoelastic forces imposed by the stiffness matrix K ≻ 0 and damping matrix D ≻ 0 . The system of matrices can then be efficiently recovered using a Matrix-Differential solver proposed in [10]. This leads to a fast evaluation of (17) as to allow for realtime simulation using precompiled mex files in Matlab. The vector Gu represents the distributed forces and torques generated by various kinds of the internal actuators (e.g., tendons or pneumatics). If rank(G) < dim(q) , the system is considered to be under-actuated. Within the context of soft robotics, whose infinite-dimensional configuration space cannot be matched by a finite number of actuators, these systems are often intrinsically under-actuated. Following the procedures in finite elements and assuming linear visco-elasticity, the stiffness matrix and damping matrix are computed through spatial integration: where K ∈ se * (3) × se(3) and D ∈ se * (3) × se(3) are the stiffness and damping tensor, respectively.

Lemma 1 The inertia matrix M(q)
is a symmetric, positive definite, symmetric. and is uniformly bounded such that there exists positive constants − ≤ + such that − I n ⪯ M(q) ⪯ + I n < ∞.
Proof Proof can be found in Spong et al. [31] ◻ Lemma 2 Given the inertia matrix M(q) and the Coriolis matrix C(q,q) as described by (18) and (19) Since the matrix J ⊤ CJ is skew-symmetric, the remainder of the proof is to show that the matrix S =J ⊤ MJ − J ⊤ MJ also satisfies skew-symmetry. Since the inertia tensor is symmetric M = M ⊤ , we can easily show this holds true: Therefore, the matrix Ṁ (q) − 2C(q,̇q) is skew-symmetric for all q,̇q ∈ ℝ n ◻ In literature, Lemma 2 is often referred to as the passivity condition [22,23,31]. It implies that, in the absence of external dissipation, the total energy of the system (i.e., the Hamiltonian) is conserved. It is also worth mentioning that this condition does not necessarily hold true for all cases, only for particular computations of the Coriolis matrix C(q,q) (e.g., through the Christoffel symbols).

Port-Hamiltonian Formulation
In this section, the Lagrangian model in (17) is rewritten in port-Hamiltonian form similar to [9]. To this end, we define the generalized momenta p ∶= Ṁq . Then, the (reduced-order) Hamiltonian is given by H(q, p) ∶= K(q, p) + U(q) with K = 1 2 p ⊤ M −1 p and U(q) the kinetic and potential energy of a reduced system, respectively.
Given the system's Hamiltonian H , it can be easily shown that generalized velocities can be written in terms of partial derivatives of the Hamiltonian function where we denote ∇ p (⋅) ∶= (⋅) ⊤ ∕ p as the gradient w.r.t. the vector field p . Similarly, we introduce ∇ q (⋅) ∶= (⋅) ⊤ ∕ q . Note that M −1 is always exists due to the positive definiteness condition in Lemma 1. Similarly, we seek a differential description that relates the time evolution of p to a statederivative of the Hamiltonian function. Applying the chain rule of differentiation to the generalized momenta: Taking the partial derivative of the Hamiltonian H w.r.t. the generalized coordinates q , we find To relate (29) and (30), we explore some structural properties in the Lagrangian model. To be more specific, we exploit the skew-symmetry condition as detailed in Lemma 2. According to the [31], if the matrix Ṁ − 2C satisfies the passivity condition in Lemma 2, it can be shown that p =Ṁq + Mq, Finally, by combining (28), (29), (30), and (31), we can show that the Lagrangian model in (17) can be equivalently rewritten as a port-Hamiltonian (pH) system: The advantage of the port-Hamiltonian model over the standard EL structure in (17) is the general applicability to different physical domains and the common formalism with energy-based control, which we will explore further in the next section.

Controller Design
Given the previous reduced-order dynamic model, our objective is to find a controller u that ensures lim t→∞ g(L, t) = g d in which g d ∈ SE(3) denotes the desired configuration of the end-effector. To achieve the control objective, the main idea here is to reshape the potential energy function of the finite-dimensional system using a standard energy-shaping techniques which are common to port-controlled Hamiltonian models [24,28].

Energy-Shaping Controller
We adopted an energy-based control strategy akin to the work of [9,17,23,24,28]. Following a similar energy-shaping strategy, the model-based nonlinear controller takes the form: where G + = G ⊤ G −1 G ⊤ is the generalized inverse of the actuation map G , and the desired Hamiltonian in closed loop H d = 1 2 p ⊤ M −1 p + U d that satisfies argmin g L U d = g d with g L = g(L, ⋅) the pose of the end-effector. Note that we purposefully omitted any damping injection as the system's intrinsic visco-elastic damping is deemed sufficiently large to guarantee stability. Following the concept of a kinematic feedback controller [4,6] that artificially mimic an elastic element between the end-effector and the desired configuration in SE(3) , we have choose the gradient of the desired potential energy as where 1 > 0 is a proportional gain, 2 > 0 a controller gain related to the damping of the pseudo-inverse, F u = k p T SE(3) (E)E an artificial control wrench with the positive definite stiffness matrix k p , E = log SE(3) ([g L ] −1 k g d ) where the mappings log SE(3) (⋅) and T SE(3) (⋅) denote the logarithmic map and the tangent-space map, respectively. We refer the reader to [30] for the numerical computations of these geometric operations. The vector E may be regarded as the geometric error between the homogeneous transformations g d and g L such that if g d = g L will simply yield ‖E‖ 2 = 0 . Furthermore, the controller gains 1 and 2 can be tuned accordingly to tweak the desired transient behavior of the closed-loop system, similar to a classical PD controller.

Model and Solver Setting
In this section, we detail the numerical simulations of the port-Hamiltonian model in (32) together with the energyshaping controller in (33). For all numerical simulations, we consider a truncation degree of the finite-dimensional model is k = 8. Due to the partial differential nature, we have to employ a nested ODE routine to recover the trajectories for q and p . First, we employ an implicit trapezoidal solver with a fixed stepsize of dt = 30 ms to solve (32). At each time increment, we have to evaluate the dynamic matrices (18)- (22). To efficiently compute these dynamic entities, we solve the spatial integration problem over the material domain using a second-order Runge-Kutta solver. The stepsize for the spatial solver is d = 5 mm. All simulation examples and underlying source code are provided publicly on the SOROTOKI toolkit [7] written in Matlab. Here, the numerical integrations of the system matrices (18), (19), (21) and (22) are performed using a so-called Matrix-Differential solver (see [10]). The simulations are performed on a modern machine (Ryzen 7-5800H CPU @3.2GHz).
For the soft robotic simulations, we have chosen a linear isotropic Hookean material with shear constraints. Although hyper-elastic material models are possible, e.g, Neo-Hookean or Yeoh, it is considered out of the scope of this work (see [19,25]). Given these material properties, the inertia tensor and the stiffness tensor become diagonal matrices: where the (average) cross-sectional area, and J the second moment of area for a disc with radius R. The damping tensor chosen as D = K with damping coefficient . The stiffness and damping matrix are precomputed using (23) and (24).

Soft Robot Manipulator Inspired by Octopus' Tentacle
In the first study case, we consider a soft robotic arm that is loosely inspired by the tentacles of an octopus. To introduce the under-actuation typically present in soft robotics, we have chosen an actuation matrix G = blkdiag I 5 , O 3 such that only the first five modes are actively controllable. The system properties can be shown in Table 2.
The soft robot is subjected to the energy-based controller in (33), where the control gains are tuned to produce a smooth transient: 1 = 0.01 and 2 = 0.001 . The artificial M = blkdiag{ J, A, A, A}, Fig. 2 Three-dimensional evolution of the soft robot manipulators, slowly converging to the desired set-point g d ∈ SE(3) (indicated by the pink ball). Please observe the morphology that arises which can be related to the motion of an octopus Table 2 Parameters setting for the numerical solver, the soft manipulator, and the energy-based controller spring stiffness is chosen as k p = blkdiag 0.01 ⋅ I 3 , I 3 .
Lastly, the desired configuration of the end-effector is chosen as follows: The numerical results of the closed-loop system are shown in Figs. 2 and 4. It is worth mentioning that these simulation run at ±60 Hz real-time sampling frequency. Real-time bandwidth is determined by the ratio between finite horizon and CPU's computation time, i.e., f ≈ T∕T sim . This ratio is affected most by spatial discretization. Figure 2 shows the evolution of the continuous deformation along the soft robotic body, whereas Fig. 4 shows the modal coefficients q(t) and the spatial trajectory of the end-effector. As can be seen, the end-effector of the soft robot manipulator slowly converges to the desired set-point g d ∈ SE(3) . Although the control gains could be increased to promote a faster transient, it was observed that high gains lead to undesired (propagating) oscillations of the flexible structure. A possible solution might be to introduce negative damping to the controller Hamiltonian H d , to overcome the soft robot's structural damping. For the second simulation run, we modified the control gains to highlight an interesting property of the proposed controller. To be more specific, we increase the controller gains to 1 = 2 = 0.1 . The numerical results for the increased controller gains are shown in Figs. 3 and 5. Although the control goal and the initial conditions are chosen identical, the soft robot converges to a different configuration-albeit, a shape with less 'complexity'. The Fig. 3 Three-dimensional evolution of the soft robot manipulators, slowly converging to the desired set-point g d ∈ SE(3) (indicated by the pink ball). Please observe that a different morphology arises due to higher control gains,i.e., 1 = 2 = 0.1 , which is caused by the controller affecting the structural compliance of the soft robot  cause of less complicated bending patterns has two origins. First, increasing the control gains also artificially impacts the structural stiffness of the soft robot, resulting in a soft robot with higher perceived stiffness. Second, by increasing the stabilizing term 2 in the damped Jacobian inverse (34), more weight is given towards finding a solution that also minimizes the join angles ‖q‖ 2 . As intrinsically, more energy is spent to excite higher-order modes in the basis { n } k n=1 , the energy-based controller will thus find a minimizer that accounts for the ordering of the shape basis, penalizing higher-order modes. This result indicates that the proposed controller can be effectively tuned alter the structural compliance of the soft robot; and thus could be implemented carefully to preserve 'softness'.

Multi-Link Soft Robot Inspired by the Elephant's Trunk
In the second study case, we consider a two-link soft robot that is inspired by the trunk of an elephant. A similar soft robotic system is considered in [16], where mobility of the bio-inspired robotic system is achieved through a pneumaticnetwork distributed along the continuous body of the robot. Therefore, considering a six-bellow network, the actuation matrix takes the form: where {F n } 6 n=1 is a set of piece-wise constant wrench functionals related to the pneumatic actuation bellows distributed along the soft robotic body, and u = (u 1 , u 2 , u 3 , u 4 , u 5 , u 6 ) ⊤ a vector of wrench amplitudes. The control input sets {u 1 , … , u 3 } relate to the first link and {u 4 , … , u 6 } to the second link of the robot. Given this input configuration, it also follows that rank(G(q)) < dim(q) for all q ∈ ℝ 6k , i.e., underactuated. The system and solver properties are given in Table 3; and we consider k = 8 spatial modes.
Again we apply the energy-based controller in (33) to the system, where the control gains are 1 = 5 and 2 = 1 , while the artificial stiffness matrix k p is kept identical to previous simulations. Lastly, the desired configuration of the endeffector is chosen as follows: The numerical results of the closed-loop system are shown in Figs. 6 and 7; which could reach a real-time performance of ±26 Hz. Figure 6 shows the continuous deformation along the soft robotic body, whereas Fig. 7 shows the trajectories of the modal coefficients q(t) and the spatial trajectory of the end-effector. As can be seen, the end-effector of the soft robot manipulator slowly converges to the desired set-point g d ∈ SE(3) , even when dealing with piece-wise distributed actuation loads applied to the continuous backbone. To describe the discontinuous actuation profiles, however, higher order modes are required as can be seen in Fig. 7. This might indicate there exist better tailored compact shape bases for this soft robotic system.

Conclusion
The field of soft robotics is slowly maturing into a recognized subfield of robotics. Due to their intrinsic compliance, they allow for complex morphological motions that Fig. 6 Three-dimensional evolution of the soft robot inspired by the elephant's trunk (whose muscular network is mimicked through six pneumatic bellows), slowly converging to the desired set-point g d ∈ SE(3) (indicated by the pink ball) mimic animals in nature. Achieving similar performance to biology highlights the need for more accurate dynamic models and control strategies that fully exploit the hyperredundant nature of soft robots. In this work, we provided a modeling framework for Cosserat beams that leads to a finite-dimensional system in a port-Hamiltonian structure. By exploiting the passivity, an energy-shaping controller was proposed that ensures the closed-loop Hamiltonian is minimal at the desired set-point. The numerical model was developed for a several bio-inspired soft robot (octopus' tentacle and elephant's trunk) with distributed control inputs.
The key challenges here regarding both the model as the controller are their ability to capture the hyper-flexibility, deal with inherent under-actuation, and exploit its hyperredundancy to achieve its control task. Given appropriate controller gains, the model-based controller yields smooth convergence of the soft robot's end-effector while accounting for under-actuation. It was shown that by tuning the controller gains, the intrinsic stiffness of the soft body can be adapted, resulting in significantly different quasi-static joint solutions to the set-point problem. To some extent, the mobility of the Cosserat model paired with the energy-based control has a (close) resemblance to biological motion. There are, however, a few limitations to our approach. The strain parametrization of functional basis does not account for the geometry of the soft robot, meaning some systems require many spatial modes to accurately represent the true continuum dynamics. Second, regarding implementation, measuring these spatial modes in an experimental setting is difficult, and future research is required to find a suitable 'soft sensing' technique that (i) has limited impact on the dynamics, and (ii) accounts for the continuity of the elastic body. A possible solution might be the optimal placement of a network of distributed localized sensors, e.g., strain gauges or IMUs. Furthermore, the proposed controller is only suited for set-point regulation or slow-varying references. Exploring (fast)-dynamic control objectives will likely require more research. In particular, controllers that suppress natural resonances of continuum elastic body under fast motion. One could argue that this perhaps fights against the natural dynamics of the soft robot, yet such oscillations might be able to be explored for locomotion or soft manipulators throwing objects rather than traditional pick-and-place strategies. Given these limitations, future work will focus on the following: (i) adding hyper-elasticity (ii) validating the controller experimentally, and (iii) constructing a set of basis functions through the so-called 'snapshot decomposition method' using FEM-driven data. In particular, the latter two goals could be interesting to explore. Both advantages in FEM and Cosserat models, being accurate continuum deformations and computational efficiency; leading to a modeling strategy for optimal finite-dimensional state projection with the insightful structure of the passive and active joints.

Continuous Kinematics
For completeness, we introduce the adjoint action on the group and its algebra by respectively. Under Assumption 1, the configuration space of the soft robot g is everywhere differentiable. Then, using the equality of mixed partials, i.e. t ( g ) = ( g t ) , we substitute g∕ t = ĝ and g∕ = ĝ to find Pre-multiplying with g −1 ∈ SE(3) and rearranging the equality above, we obtain where we can recognize the Lie bracket or the commuter between the vector fields and [22]. Since the Lie bracket [̂,̂] itself also belongs to se(3) , which is isomorphic to ℝ 6 via ̂→ , we can rewrite the expressions as follows