An efficient and direct method for trajectory optimization of robots constrained by contact kinematics and forces

Abstract

In this work, we propose a trajectory generation method for robotic systems with contact kinematics and force constraints based on optimal control and reachability analysis tools. Normally, the dynamics and constraints of a contact-constrained robot are nonlinear and coupled to each other. Instead of linearizing the model and constraints, we solve the optimal control problem directly to obtain feasible state trajectories and their corresponding control inputs. A tractable optimal control problem is formulated and subsequently addressed by dual approaches, which rely on sampling-based dynamic programming and rigorous reachability analysis tools. In particular, a sampling-based method together with a Partially Observable Markov Decision Process solution approach are used to break down the end-to-end trajectory generation problem by generating a sequence of subregions that the system’s trajectory will have to pass through to reach its final destination. The distinctive characteristic of the proposed trajectory optimization algorithm is its ability to handle the intricate contact constraints, coupled with the system dynamics, in a computationally efficient way. We validate our method using extensive numerical simulations with two legged robots.

Appendices

Appendices

Proof of Proposition 1

Proof

Since the samples are uniformly distributed, it is possible to select any unit vector in $\mathbb {R}^{n_y}$ as the PSV of $s_i$, that is, $\mathbf {R}_a\mathcal {V}(s_i)$ where $\mathbf {R}_a \in \mathrm {SO}(3)$ is a rotation matrix. If we select the rotation matrix $\mathbf {R}_a$ such that $d^{\perp } = \mathbf {R}_a \mathcal {V}(s_i)$, which is orthogonal to $\mathcal {V}(s_i)$, it follows that $\mathcal {T}(s',s,a) = \langle \mathcal {V}(s_{i}), d \rangle = \langle d^{\perp }, d \rangle = 0$ for all $a \in {\mathcal {A}}$. $\square$

Proof of Corollary 1

Proof

Let us consider a general hull of the set $\mathcal {R}_y(T_d^{\varDelta t}, x_0)$, that is $\mathrm {ghull}(\mathcal {R}_{y}^{D}(T_d^{\varDelta t},x_0))$, being compact. By the Heine− Borel theorem, all closed subsets of a compact set are also compact. Since $\mathcal {R}_{y}(T_d^{\varDelta t},x_0) \subset \mathrm {ghull}(\mathcal {R}_{y}(T_d^{\varDelta t},x_0))$, the reachable set $\mathcal {R}_{y}(T_d^{\varDelta t},x_0)$ is compact. $\square$

Proof of Corollary 2

Proof

Consider three sets: $\mathcal {F}_{1} = \mathcal {R}_{x}(T_d^{\varDelta t}, x_0)$, $\mathcal {F}_{2} = \mathcal {R}_{x}(t_k, x_0)$, and $\mathcal {F}_{3} = \mathcal {F}_2 \cup \mathcal {F}_{1}'$, where $\mathcal {F}_{1}'$ is the collection of states $x\in \mathcal {F}_1$ producing the next feasible state by Definition 5 with respect to $x \in \mathcal {F}_{1}$. It is true that $\mathcal {R}_{x}(T_d^{\varDelta t}, x_0)=\mathcal {F}_1\cup \mathcal {F}_2 = \mathcal {F}_{2} \cup \mathcal {F}_{3}$. Let us consider arbitrary two sets $\mathcal {H}_1$ and $\mathcal {H}_2$ satisfying $\mathcal {R}_{x}(T_d^{\varDelta t}, x_0) = \mathcal {H}_1 \cup \mathcal {H}_2$ with $\mathcal {H}_1 \cap \mathcal {H}_2 = \emptyset$. Let $x \in F_{1}'$ and suppose $x \in \mathcal {H}_{1}$. Then, $\mathcal {H}_1 \cap \mathcal {F}_{1} \ne \emptyset$ and $\mathcal {H}_1 \cap \mathcal {F}_{3} \ne \emptyset$. This implies that $\mathcal {F}_{1} \subseteq \mathcal {H}_1$ and $\mathcal {F}_{3} \subseteq \mathcal {H}_1$, hence, $\mathcal {H}_{2} = \emptyset$. This proves that $\mathcal {R}_{x}(T_d^{\varDelta t}, x_0)$ is connected. Since the mapping $f_y$ is continuous, we also conclude that the set $\mathcal {R}_{y}(T_d^{\varDelta t}, x_0)$ is connected. $\square$

Proof of Theorem 1

Proof

Since $\mathcal {R}_{y}(T_d^{\varDelta t}, x_0)$ is compact and $\hat{f}_y$ is continuous, $\mathcal {R}_{x}(T_d^{\varDelta t}, x_0)$ is closed and $\hat{f}_y^{-1}$ is also continuous. Then, $\mathcal {R}_{x}(T_d^{\varDelta t}, x_0)$ is connected because $\mathcal {R}_{y}(T_d^{\varDelta t}, x_0)$ is connected and $\hat{f}_y^{-1}$ is continuous. Therefore, there exists at least one trajectory connecting $x_0$ to $x(\tau )$ satisfying $f_y(x(\tau )) = y^g$ in $\mathcal {R}_{x}(T_d^{\varDelta t}, x_0)$. $\square$

Specifications of Valkyrie

We consider the following joint position/velocity/torque constraints. Excluding the virtual joints for the floating base, the actuated joints ($\mathbb {R}^{28}$) are specified such as

$$\begin{aligned} \begin{aligned} q_{init} =&[ 0.0,\, 0.0,\, 0.0, \, 0.0, \, 0.0, \, 0.0, \, 0.2, \, \\&1.1, \, 0.0, \, 0.4, \, 1.5, \\&0.0,\, 0.0,\, -0.6, \, 1.2, \, 0.6, \, 0.0, \\&0.0,\, 0.0,\, -0.6, \, 1.2, \, 0.6, \, 0.0 ],\\ q_{UB} =&[1.181, \, 0.666, \, 0.255, \, 1.162, \, \\&1.047, \, 0.0, \, 2.0, 1.519, \\&2.18,\, 2.174,\, 3.14, \,2.0, \, 1.266, \, 2.18, \, 0.12, \, 3.14,\\&0.4141, \, 0.467, \, 1.619, \, 2.057, \, 0.875,\, 0.348,\, 1.1, \\&0.5515, \, 1.619, \, 2.057, \, 0.875, \, 0.348 ],\\ q_{LB} =&[0.0, \, -1.047, \, -0.872, \, -2.85, \, \\&-1.266, \, -3.1, \, -0.12, \\&-2.019, \, -2.85, \, -1.519, \, -3.1, \, \\&-2.174, \, -2.019, \, -1.1,\\&-0.5515, \, -2.42, \, -0.083, \, -0.8644, \\&\, -0.349, \, -0.4141, \\&-0.467, \, -2.42, \, -0.083, \, -0.8644, \, -0.349 ],\\ \dot{q}_{B} =&[ 5.29, \, 9.0, \, 9.0, \, 5.0, \, 5.0, \, \\&5.0, \, 5.89, \, 5.89, \, 11.5, \, 11.5, \\&5.0, \, 5.89, \, 5.89, \, 11.5, \, 11.5, \, 5.0, \, \\&5.89, \, 7.0, \, 6.11, \, 6.11, \\&11.0, \, 11.0, \, 5.89, \, 7.0, \, 6.11, \, 6.11, \, 11.0, \, 11.0 ],\\ u_{B} =&[ 150.0, \, 150.0, \, 150.0, \, 26.0, \, 26.0, \, \\&26.0, \, 190.0, \, 190.0,\\&65.0,\, 65.0,\, 14.0,\, 190.0,\, 190.0,\, 65.0,\, \\&65.0,\, 14.0,\, 190.0, \\&350.0, \, 350.0, \, 350.0,\, 205.0,\, 205.0, \, 190.0, \\&350.0, \, 350.0, \, 350.0, \, 205.0, \, 205.0]; \end{aligned} \end{aligned}$$

where $\dot{q}_{UB} = + \dot{q}_{B}$, $\dot{q}_{LB} = - \dot{q}_{B}$, $u_{UB} = + u_{B}$, and $u_{LB} = - u_{B}$, respectively.