# Retrieving contact points without environment knowledge

Sebastien Lengagne, Omer Terlemez, Sophie Laturnus, Tamim Asfour, Rudiger Dillmann

► To cite this version:

Sebastien Lengagne, Omer Terlemez, Sophie Laturnus, Tamim Asfour, Rudiger Dillmann. Retrieving contact points without environment knowledge. 2012 12th IEEE-RAS International Conference on Humanoid Robots (Humanoids 2012), Nov 2012, Osaka, Japan. pp.841-846, 10.1109/HUMANOIDS.2012.6651618 . hal-03935672

HAL Id: hal-03935672

https://hal.science/hal-03935672v1

Submitted on 29 Sep 2024

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Retrieving Contact Points
Without Environment Knowledge

Sébastien Lengagne, Ömer Terlemez, Sophie Laturnus, Tamim Asfour, Rüdiger Dillmann
Institute for Anthropomatics, Karlsruhe Institute of Technology (KIT), Karlsruhe, Germany

***Abstract***—This paper paves the way for contact retrieving of human motions without environment knowledge. The goal is to find out the minimal set of contacting links of the human body, that is required to perform a recorded motion. First, we fit the captured motion to a unified representation of the human: the Master Motor Map. Looking at the Minimal Oriented Bounding Boxes of the velocity and acceleration for every link, we determine whether one part of the link is moving or not. This provides an initial guess of the contacting links. Then, based on the dynamic equations of the model, we find the minimal set of contacting links that ensure the balance. Eventually, we assess this method on several motions with actual and pretended contacts. We show that it is efficient for motions such as walking and that it deserves to be improved for more complex motions with a lot of contact points.

***Index Terms***—Motion capture, balance, contact force, Master Motor Map, Kinematic Bounded Boxes, human models.

## I. INTRODUCTION

In order to reproduce human motions on a humanoid robot, one has to look at the contacts between the human subject and the environment. This is essential to reproduce the same sequence of contact stances. We consider that the environment cannot be known in an accurate way for every situation. Hence, we get rid of any environment knowledge and focus our method on retrieving the contacting links during human motions from kinematic and dynamic properties.

Human motion capture was already used to fit dancing motions on humanoid robots [1]. It was also used to reconstruct the muscle activity using additional EMG and measuring the contact forces [2]. Recent work focus on the estimation of the dynamic properties of the contact [3] or of the human body [4].

To transfer a human motion to a humanoid robot, some methods were already proposed such as the Dynamics Filter [5]. In this paper, we use the framework of the Master Motor Map as introduced in [6], [7] and presented in Figure 1. Starting from any Human Motion Capture (HMC), it transfers the motion into a unified representation of the human body that can be converted to any kind of robot or virtual avatar in a second step. We focus on the computation of the contact stances of the unified representation that are needed to reproduce the motion on an actual robot.

In this paper we use the Master Motor Map (MMM) as the unified representation of the human. From this representation, we propose a method as presented in Algorithm 1 to retrieve the contacting links. First, we briefly present how to fit the motion to the MMM model in Section II. Then, we

<figure><img src="image_2.png" /><figcaption>Fig. 1. Representation of the global framework of the transfer from motion capture to humanoid robot. Our work is about the contact retrieving in the unified representation of the human motion: the Master Motor Map (see [6], [7]).</figcaption></figure>

present our first contribution on the Minimal Bounded Boxes (MOBB) of the velocity and acceleration of every link to determine if they are kinematically suitable to be in contact in Section III. Section IV presents the second contribution on the probability of links to be in contact regarding their impact on the balance of the subject. Eventually, we define the minimal set of contacting links over small time intervals. We validate our method with several motions as depicted in Section V.

**Algorithm 1** Description of the contact retrieving algorithm

Require: captured marker positions

1. Kinematic fitting of the motion to the MMM

2. computation of the kinematic suitability / link ranking

3. computation of the impact of the links on the balance

4. retrieving the contacts over small time intervals

## II. HUMAN MOTION TO MMM

The first step of our method is to fit the captured data to the reference MMM Model. Thus, we describe the MMM Model and the optimization process that is used to fit the motion and discuss the issue of the orientation of the reference body.

### A. Master Motor Map

In this paper, we use the Master Motor Map (MMM) which was first proposed in [6] and [7] as a unified representation of the human model. This model has eighteen spherical joints, thus fifty four degrees of freedom as presented in Figure 2. The kinematics and dynamic properties of this model are set according to the size and mass of the subject
using DeLeva anthropometrics parameters as presented in Table I from [8]. The length *L*, mass *M*, the Center Of Mass COM and the Radius Of Girdle ROG of any segment are given as a percentage of the total human size *S* and weight *W*.

Moreover, we consider the joint limits as presented in Table II.

<figure><img src="image_3.png" /><figcaption>Fig. 2. Kinematic representation of the of the MMM model for zero joint positions.</figcaption></figure>

<table><caption>TABLE I
KINEMATIC AND DYNAMIC PROPERTIES OF THE MMM'S BODIES. THE VALUES ARE PRESENTED AS A PERCENTAGE OF BODY SIZE OR WEIGHT.</caption><thead><tr><th rowspan="2">Segment</th><th rowspan="2">L</th><th rowspan="2">M</th><th colspan="3">COM</th><th colspan="3">ROG</th></tr><tr><th>x</th><th>y</th><th>z</th><th>x</th><th>y</th><th>z</th></tr></thead><tbody><tr><td>Waist</td><td>13</td><td>11</td><td>0</td><td>4</td><td>0</td><td>34</td><td>36.5</td><td>38</td></tr><tr><td>Spine</td><td>10</td><td>10</td><td>0</td><td>46</td><td>-4</td><td>28.6</td><td>26</td><td>32</td></tr><tr><td>Chest</td><td>18</td><td>17</td><td>0</td><td>46</td><td>0</td><td>31.3</td><td>28.5</td><td>35</td></tr><tr><td>Neck</td><td>5</td><td>2.4</td><td>0</td><td>20</td><td>0</td><td>31.6</td><td>33</td><td>31.6</td></tr><tr><td>Head</td><td>13</td><td>7</td><td>0</td><td>13</td><td>-12</td><td>30</td><td>26</td><td>31</td></tr><tr><td>Shoulder<fnref n="1" /></td><td>10</td><td>21</td><td>-66</td><td>0</td><td>0</td><td>12</td><td>26</td><td>26</td></tr><tr><td>Upper arm<fnref n="1" /></td><td>16</td><td>27</td><td>0</td><td>-57.3</td><td>0</td><td>28.4</td><td>15.7</td><td>26.8</td></tr><tr><td>Lower arm<fnref n="1" /></td><td>13</td><td>16</td><td>0</td><td>-53.3</td><td>0</td><td>32</td><td>14</td><td>31</td></tr><tr><td>Hand<fnref n="1" /></td><td>10</td><td>2.1</td><td>-66</td><td>0</td><td>0</td><td>12</td><td>26</td><td>26</td></tr><tr><td>Thigh<fnref n="1" /></td><td>25</td><td>14</td><td>0</td><td>-33</td><td>0</td><td>25</td><td>11.4</td><td>25</td></tr><tr><td>Shank<fnref n="1" /></td><td>23</td><td>4</td><td>0</td><td>-44</td><td>0</td><td>26.4</td><td>10.5</td><td>25.4</td></tr><tr><td>Foot<fnref n="1" /></td><td>15</td><td>1.3</td><td>0</td><td>-6</td><td>-39</td><td>21</td><td>19.5</td><td>12</td></tr></tbody></table>

To compute the dynamic effect that occurs during a motion as presented in Section IV-B, we need the inertia *I* of each link of the human body using the following formula:

$$I = M.W.(L.S)^2 \cdot \begin{pmatrix} ROG_x & 0 & 0 \\ 0 & ROG_y & 0 \\ 0 & 0 & ROG_z \end{pmatrix} \quad (1)$$

<table><caption>TABLE II
JOINTS LIMITS FOR THE BIOMECHANICAL MODEL (IN RADIAN).</caption><thead><tr><th>Joint</th><th>θ<sub>x</sub></th><th>θ<sub>y</sub></th><th>θ<sub>z</sub></th></tr></thead><tbody><tr><td>Pelvis</td><td>-0.87 : 0.61</td><td>-0.70 : 0.70</td><td>-0.09 : 0.09</td></tr><tr><td>Mid-spine</td><td>-0.61 : 0.47</td><td>-0.34 : 0.34</td><td>-0.63 : 0.63</td></tr><tr><td>Lower neck</td><td>-1.13 : 0.70</td><td>-0.61 : 0.61</td><td>-0.61 : 0.61</td></tr><tr><td>Upper neck</td><td>-1.13 : 0.70</td><td>-0.61 : 0.61</td><td>-0.61 : 0.61</td></tr><tr><td>Clacivula<sup>2</sup></td><td>-0.01 : 0.01</td><td>-0.15 : 0.15</td><td>-0.15 : 0.15</td></tr><tr><td>Shoulder<sup>2</sup></td><td>-2.26 : 3.14</td><td>-2.26 : 0.00</td><td>-1.04 : 0.52</td></tr><tr><td>Elbow<sup>2</sup></td><td>-0.001 : 2.79</td><td>-1.57 : 1.57</td><td>-0.01 : 0.01</td></tr><tr><td>Wrist<sup>2</sup></td><td>-1.22 : 0.87</td><td>-0.01 : 0.01</td><td>-0.52 : 0.35</td></tr><tr><td>Hip<sup>2</sup></td><td>-0.87 : 1.65</td><td>-0.35 : 1.13</td><td>-0.61 : 0.61</td></tr><tr><td>Knee<sup>2</sup></td><td>-2.26 : 0.001</td><td>-0.01 : 0.01</td><td>-0.01 : 0.01</td></tr><tr><td>Ankle<sup>2</sup></td><td>-0.69 : 0.52</td><td>-0.34 : 0.34</td><td>-0.34 : 0.34</td></tr></tbody></table>

## B. Optimization Problem

During the capture session, the subject is equipped with a set of markers. The position of the equivalent markers of the reference model is:

$$^w P_{m_i} = S_i \cdot (^w P_{b_i} + ^w R_{b_i} {}^{b_i} P_{m_i}) \quad (2)$$

Where, *S* is the subject size, ${}^{b_i} P_{m_i}$ the marker position for a one-meter high subject expressed in the correspondent frame *i*, ${}^w R_{b_i}$ and ${}^w P_{b_i}$ the orientation matrix and position vector of the frame expressed in the world frame that rely on the joint values *q* and on the position and orientation ${}^w P_{ref}$, ${}^w R_{ref}$ of the reference body<fnref n="3" /> of the MMM Model. Note that, in the following of this paper, we simplify the notation and consider any position in the world frame.

The fitting of the motion to the MMM Model turns into finding the size of the subject *S*, the trajectories of the joint values, the position and orientation of the reference body $X(t) = \{q(t), P_{ref}(t), R_{ref}(t)\}$ that minimize the difference between the measured and the computed positions of the markers, ensuring that joint positions and velocities are within their appropriate limits:

$$ \begin{array}{l} \min_{X(t),S} \sum_{t_i=0}^{T} \sum_{j=0}^{m} (P_{m(t_i,j)} - P_{c(t_i,j)})^2 \\ \forall i, \forall t \in [0, T] \qquad \left\{ \begin{array}{l} \underline{q}_i \le q_i(t) \le \overline{q}_i \\ \underline{\dot{q}}_i \le \dot{q}_i(t) \le \overline{\dot{q}}_i \end{array} \right. \end{array} \quad (3) $$

Where *m* is the number of markers and *t*<sub>*i*</sub> represents the time frame of the captured data.

<footnote n="1">¹The data of this table are for the left parts, for the right parts, please, consider the opposite value for the *y*-component of the COM.</footnote>

<footnote n="2">²The data of this table are for the left parts, for the right parts, please, consider the opposite value for the limits for $\theta_y$.</footnote>

<footnote n="3">³Here we consider the waist as the reference body</footnote>
### C. B-spline parametrization

Previous work optimizes the motion using a frame by frame optimization [7], and returns to a filtering process in order to avoid high frequency motion of the joint angles. The size of the subject S was computed from the first frame from a specific posture (usually a T-pose). In this paper, we rather optimize the whole motion and the size at once. To do so, we parametrize the trajectories using third-order uniform B-spline functions [9], already used in robotics fields [10], [11]. Eventually the trajectory $\psi(t)$ is computed from $N_s$ control points:

$$ \forall t \in [0, T_f] \quad \psi(t) = \sum_{i=1}^{N_s} b_i^3(t) p_i \quad (4) $$

$\psi(t)$ can represent the joint trajectories and the trajectories of the position and orientation of the waist, $p_i$ are the control points of the B-spline. The use of B-spline functions produces smooth and continuous trajectories. In this paper, we pay attention to the trajectory of the reference body and recommend to perform a pre processing in order to avoid mistake and the angles go from $\pi - \epsilon$ to $-\pi + \epsilon$.

## III. KINEMATIC INDICATOR

### A. Principle

Since the motion kinematically fits the captured data, we try to retrieve the minimal set of contacting links. In this paper, we focus on non-sliding contacts. Hence, we look after non-moving links or parts of the links for every frame. Then, we rank the link over the whole motion regarding the amount of time for which they do not move. We use a fitting walking motion presented in Figure 3, as a tutorial example.

### B. Velocity and Acceleration Bounding Box

To determine if a link might be contacting the environment, we look if, at least, one point of this link is not moving, i.e., has its velocity and acceleration equal to zero. To do so, we compute the Minimal Oriented Bounding Box (MOBB) of the velocity and acceleration for all the points of this link, as presented in Figure 4.

Once we compute the MOBB for the velocities and accelerations, we get some clues about the existence of a contact for the current body:

* case a: the MOBBs do not contain zero: every points of the link is moving, this link is not in contact

* case b: the MOBBs contain zero: at least one point of the link is not moving, this body might be in contact

* case c,d: the MOBBs contain zero along one axis (y or u): there is at least one point of the body that is not moving along this axis, this might be a sliding contact (in y direction for the case c and in u direction in case d). In this paper, we do not consider sliding contacts which will be the topic of future works.

The second point we do not use here, but that deserves to be considered in future work, is about the size of the

<figure><img src="image_4.png" /><figcaption>Fig. 4. 2D Illustration of the 3D Minimal Oriented Bounding Box for the velocity of one link.</figcaption></figure>

MOBBs. We can have a guess of what the kind of contact (planar, linear, punctual) might be:

* the three dimensions of the MOBBs are tight: all the points of the body do not move, this might be a planar contact,

* two dimensions of the MOBBs are tight: some points move in a given direction, this might be a linear contact (rotation around one edge),

* one or zero tight dimensions: some points move in different directions, this might be a punctual contact.

### C. Kinematic Body State (KBS)

In this paper, we consider that the link i might be in contact at time t if the MOBB of the velocity $B_i^i(t)$ and of the acceleration $B_a^i(t)$ intersect the threshold boxes $[\epsilon_v] = [-\epsilon; +\epsilon]^3$ and $[\epsilon_a] = [-5\epsilon; +5\epsilon]^3$.

$$ \text{if } (B_v \cap [\epsilon_v] = \emptyset) \quad \begin{cases} \rho_i(t) = 0 \\ \text{else} \end{cases} \quad \rho_i(t) = 1 \quad (5) $$

$\rho_i(t)$ is called the Kinematic Body State (KBS) and is equal to one when the link might be in contact (at least one point does not move) and is equal to zero otherwise. Figure 5 represents the evolution of the KBS for the left foot and the right foot. From the KBS, one can see that the left foot is moving first and the motion might be composed of 2 steps.

### D. Whole motion kinematic indicator

From the KBS at each frame, we compute the indicator $\alpha_i$ which we consider as a probability of the corresponding link to be in contact during the motion:

$$ \alpha_i = \frac{\mu_i \times \tau_i}{n_i} \times \sum_{t=0}^{T} \rho_i(t) \quad (6) $$
<figure><img src="image_5.png" /><figcaption>Fig. 3. Representation of the tutorial walking motion.</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>Fig. 5. Representation of the Kinematic Body State for the left foot and the right foot of the tutorial walking motion for $\Delta t = [0; 0.1]$.</figcaption></figure>

Where $\mu_i$ is the initial probability of the link as defined in Table III, $\tau_i$ is the longest amount of time for which $\rho_i(t) = 1$ and $n_i$ is the number of changes for the value of $\rho_i(t)$.

<table><caption>TABLE III INITIAL CONTACT PROBABILITY FOR EACH BODY.</caption><thead><tr><td>Body</td><td>μ<sub>init</sub></td></tr></thead><tbody><tr><td>right/left foot</td><td>1</td></tr><tr><td>right/left hand</td><td>0.1</td></tr><tr><td>waist</td><td>0.01</td></tr><tr><td>right/left thigh</td><td>0.001</td></tr><tr><td>right/left shank</td><td>0.01</td></tr><tr><td>right/left forearm</td><td>0.001</td></tr><tr><td>right/left arm</td><td>0.01</td></tr><tr><td>chest</td><td>0.0001</td></tr><tr><td>head</td><td>0.0001</td></tr><tr><td>neck</td><td>0.0001</td></tr></tbody></table>

Then, we rank the links regarding the value of $\alpha_i$, for which the link $a_1$ has the highest indicator value. The final ranking of our tutorial example is presented in Table IV.

<table><caption>TABLE IV KINEMATIC RANKING FOR THE TUTORIAL WALKING MOTION.</caption><thead><tr><td>rank</td><td>body</td><td>value of α<sub>i</sub></td></tr></thead><tbody><tr><td>a<sub>1</sub></td><td>right foot</td><td>292681</td></tr><tr><td>a<sub>2</sub></td><td>left foot</td><td>119465</td></tr><tr><td>a<sub>3</sub></td><td>left hand</td><td>1596</td></tr><tr><td>a<sub>4</sub></td><td>right hand</td><td>1540</td></tr><tr><td>...</td><td>...</td><td>...</td></tr></tbody></table>

From Table IV, it appears that the feet are more suitable to be in contact with the environment than the other parts of the body.

## IV. TAKING THE DYNAMICS INTO ACCOUNT

The final step of our method is to take into account the dynamic effects of the motion. We aim at finding the minimal

set of contacting links that ensure the balance of our model.

### A. Dynamic model and balance

The most commonly used criteria to characterize the balance of humanoid robots is the Zero Moment Point [12]. Unfortunately, this method cannot be applied in case of non-planar contact. We rather characterize the balance of the robot by monitoring if the Contact Wrench Sum (CWS), due to gravity and dynamic effects, remains in the Contact Wrench Cone (CWC) as presented in [13]. We start from the inverse dynamic model:

$$ \begin{bmatrix} \Gamma \\ 0 \end{bmatrix} = \begin{bmatrix} \mathbf{M}_1(q) \\ \mathbf{M}_2(q) \end{bmatrix} \ddot{q} + \begin{bmatrix} \mathbb{H}_1(q, \dot{q}) \\ \mathbb{H}_2(q, \dot{q}) \end{bmatrix} + \begin{bmatrix} \mathbf{J}_1^T(q) \\ \mathbf{J}_2^T(q) \end{bmatrix} \mathbf{F} \quad (7) $$

where $\Gamma \in \mathbb{R}^{N_{dof}}$ is the vector of the joint torques, $\mathbf{M}_1 \in \mathbb{R}^{N_{dof} \times N_{dof}}$, $\mathbf{M}_2 \in \mathbb{R}^{6 \times N_{dof}}$ are the two components of the inertia matrix, $\mathbb{H}_1 \in \mathbb{R}^{N_{dof}}$ and $\mathbb{H}_2 \in \mathbb{R}^6$ are the two vector components due to gravity, centrifugal and Coriolis effects, $\mathbf{J}_1 \in \mathbb{R}^{N_{dof} \times 3N_f}$ and $\mathbf{J}_2 \in \mathbb{R}^{6 \times 3N_f}$ are the components of the Jacobian matrix, $\mathbf{F}$ is the vector of the contact forces and $q \in \mathbb{R}^{N_{dof}}$ is a vector containing the $N_{dof}$ joint positions ($q_i$).

Considering $\mathbf{F} = \{F_1, F_2, \dots, F_{N_f}\}$ as a set of $N_f$ linear forces, the balance of the robot will be satisfied, if the contact forces that counterpart the dynamics effects are unilateral and stay within the friction cone, i.e. the contact forces must ensure the following constraints:

$$ \begin{gathered} \mathbb{D}_2 + \mathbf{J}_2^T \mathbf{F} = 0 \\ \forall i \in \{1, \dots, N_f\} \qquad F_i^n > 0 \\ \forall i \in \{1, \dots, N_f\} \qquad \|F_i^t\|^2 \le \sigma_i^2 F_i^{n2} \end{gathered} \quad (8) $$

where $D_2 = \mathbf{M}_2(q)\ddot{q} + \mathbb{H}_2(q, \dot{q})$ is the force due to the dynamic effects applied on the reference body, and $\sigma_i$ is a guess of the friction coefficient.

### B. Does contacting link help?

In this subsection, we define a criterion $C_r$ ($r \in \mathbb{N}^+$) which declares if the set of contacting links $a_1$ to $a_r$ is sufficient to ensure the balance. This criterion is computed for any frame and will be used to retrieve contacts over a longer time interval as depicted in Section IV-C.

Starting from the ranking of the bodies and the KBS, we will evaluate the impact of the link $a_1$ to $a_r$ on the balance of the robot. To do so, we will evaluate how the balance of the body can be ensured, i.e., how much additional moment
must be considered on the non-moving contacting links to counterpart the dynamic effects. Hence, considering the r first links of the kinematic ranking, we solve the following problem:

$$
\begin{equation}
\begin{array}{ll}
\min_{F_i, M_i, P_i} & C_r = \sum_{i=a_1}^{a_r} \rho_i M_i^2 \\
\text{with} & \sum_{i=a_1}^{a_r} \rho_i (J_i^T (P_i) [F_i M_i]^T) = -\mathbb{D}_2 \\
\text{and} & \forall i \in \{a_1, \dots, a_r\} \quad P_i \in V_i
\end{array}
\tag{9}
\end{equation}
$$

where, $M_i$ is the additional moment, $P_i$ is the 3D-position of the contact force and $V_i$ is the volume of the link $i$. The criterion $C_r$ of problem (9), indicates the capacity of the considering links to be contacting the environment. Since we consider linear forces, we consider that no extra moments are needed to ensure balance when $C_r \approx 0$.

As stated previously, this paper paves the way for retriev-
ing contacts without environment knowledge. Obviously, the
contact point Pᵢ should not be within the volume of the link
but on the surface of this volume. This point will be included
in future work. To describe the non contacting phase, we
define the criterion C₀ as the sum of the dynamic effects
expressed in the frame of the reference body:

$$
C_0 = \|^{\text{ref}} \mathbb{D}_2\|^2 \quad (10)
$$

C. Contact phase retrieving

The last step of our method is to determine the successive
contact phases for the whole motion. We decompose the
motion into several intervals of Δt = 0.1s. On each interval
we sum the indicators I<sub>r</sub> = &Sigma;<sub>t<sub>i</sub> &isin; &Delta;<sub>t</sub></sub> C<sub>r</sub>(t<sub>i</sub>) in order to find
the minimal set r of contacting points.

<figure><img src="image_7.png" /><figcaption>Fig. 6. Representation of the indicator $I_r$ over the whole motion duration for the tutorial walking motion.</figcaption></figure>

Figure 6 shows the computation of the indicator $I_r$ over the
whole motion duration for the tutorial walking example. The
sole usage of the right foot is not sufficient, but considering
the right foot and the left foot ($r = 2$) makes this indicator
very close to zero. We conclude that during this motion the
right and left feet are the minimal contacting links that are

**Algorithm 2** Determine the minimal set of contacting links
$r$ from the indicators $I_r$

Require: $I_r$

1: **if** $I_0 < 0.01 * 9.81 * W$ **then**

2: There is no contact

3: End of the algorithm

4: end if

5: $r = 1$

6: **while** $I_r \le I_{18} + 0.01(I_0 - I_{18})$ **do**

7: $r = r + 1$

8: end while

9: the minimal set is composed of the links $a_i$ is $\rho_i = 1$

needed, therefore they must be the only contacting links for
this motion. To define the minimal set of contacting links,
we perform the algorithm as presented in Algorithm 2.

$I_{18}$ is the minimal value of the criterion when considering all the 18 bodies. Ideally $I_{18}$ must be zero. However, if the KBS was too restrictive, the process might ignore some needed bodies, which will produce $I_{18} > 0$. Eventually, we get the minimal set of contacting links that counterpart at least ninety nine percent of the dynamic effects. The final results of contact retrieving on the tutorial walking motion is presented in Figure 7.

V. EXPERIMENTS

We evaluated our method on several motions: walking
forward, walking backward, stepping with leaning on a chair,
stepping while pretending to lean on a chair, crawling and
making a cart wheel motion. We used the optimization
solver Ipopt [14] to fit the motions to the MMM-Model and
considered the threshold $\epsilon = 0.1$ for the kinematic part of
the contact retrieving. The results are shown in the attached
multimedia file and can be found at http://www.youtube.com/
watch?v=FRIYmZDInxE.

The contact retrieving is very effective for simple motions
such as walking. Unfortunately, there are some errors for
complex motions, where our method retrieved some links
as contacting (non-contacting) despite they obviously were
(weren't). We consider, that without any environment knowl-
edge, our method is good enough to provide a guess for the
contacting links even if a manual check may still be required
in the general case.

VI. DISCUSSION

Our method proved to be efficient for some motions such
as walking and stepping pretending or actually leaning on a
chair. Nevertheless, for more complex motions this method
can be used as an initial guess of the actual solution that has
to be (for now) corrected manually.

We set the initial contact probability (cf. Table III) to give
high priority to the feet and hands. This might fit most of the
situations. However, for some specific motions this can lead
to a wrong contacting sequence. In this case, the results or the
initial contact probabilities deserve to be checked manually.
<figure><img src="image_8.png" /><figcaption>Fig. 7. Representation of the contacting links for the tutorial walking motion.</figcaption></figure>

As depicted in Section IV-B, we are looking for contact points that are within the shape of the links. The next step of our work will be to research contact points that are on the surface of the links. This research can also be restricted to the area of the link that does not move. Currently, we detect that one part of the link does not move and look for the contact point within the whole shape of the link. It is also interesting to ignore the points of some parts of the links, for example the parts that are merged with another body (the shank and the ankle for instance) and to take into account the friction cone normal to the shape. To improve our method, it might be interesting to build a feasibility map of the environment, i.e., any point of the world that was occupied with a part of the body cannot be a possible point of contact, hence is not in the feasibility map. This will only work in a static environment.

In this paper, we based our contact indicator only on balance. It could be also interesting to take into account the torque limits of the model in order to validate the balance and if the motion can be performed by a human. One can also think about an evaluation of how the motion can be done in a comfortable way through this contact stance.

The last improvement should be to automatically modify the threshold of the KBS, in such way that we can find the perfect balance, i.e., the indicator $C_r$ equals zero. This should overcome the error in velocity and acceleration due to the motion fitting.

## VII. CONCLUSION

We proposed a method to retrieve the contact points of a captured motion without any environment knowledge. First, we generated a motion for the MMM Model, our human reference model, that minimizes the error between the measured marker positions and the equivalent ones attached to the MMM model. Then, we defined the Kinematic Body State (KBS) to describe if a link is moving or not. For the computed KBS, we rank the links regarding their probability to be in contact. Eventually, we studied the impact of the links on the balance of our model, and found the minimal set of the contacting links. We evaluate our method with several scenarios. Despite some mistakes, we consider that our results are effective in retrieving the contacting links of a captured motion without any environment knowledge.

## ACKNOWLEDGEMENT

The research leading to these results has received funding from the European Union Seventh Framework Programme under grant agreement no. 270273, (Xperience) and from the German Research Foundation (DFG: Deutsche Forschungs-gemeinschaft) under the SFB 588.

## REFERENCES

[1] S. Nakaoka, A. Nakazawa, K. Yokoi, H. Hirukawa, and K. Ikeuchi, "Generating whole body motions for a biped humanoid robot from captured human dances," in *IEEE International Conference on Robotics and Automation*, vol. 3, Sep. 2003, pp. 3905-3910.

[2] Y. Nakamura, K. Yamane, Y. Fujita, and I. Suzuki, "Somatosensory computation for man-machine interface from motion capture data and musculoskeletal human model," *IEEE Transactions on Robotics*, vol. 21, pp. 58-66, 2005.

[3] M. A. Brubaker, L. Sigal, and D. J. Fleet, "Estimating contact dynamics," in *IEEE International Conference on Computer Vision*, 2009.

[4] K. Ayusawa, G. Venture, and Y. Nakamura, "Real-time implementation of physically consistent identification of human body segments," in *ICRA*, 2011, pp. 6282-6287.

[5] K. Yamane and Y. Nakamura, "Dynamics filter - concept and implementation of on-line motion generator for human figures," *IEEE Transactions on Robotics and Automation*, vol. 19, no. 3, pp. 421-432, 2003.

[6] P. Azad, T. Asfour, and R. Dillmann, "Toward an unified representation for imitation of human motion on humanoids," in *IEEE International Conference on Robotics and Automation*, Rome, Italy, April 2007, pp. 2558-2563.

[7] S. Gärtner, M. Do, C. Simonidis, T. Asfour, W. Seemann, and R. Dillmann, "Generation of human-like motion for humanoid robots based on marker based motion capture data," in *41st International Symposium on Robotics*, 2010.

[8] P. de Leva, "Adjustments to zatsorsky-seluyanov's segment inertia parameters," *J. of Biomechanics*, vol. 29, no. 9, pp. 1223-1230, 1996.

[9] C. De Boor, *A Practical Guide to Splines*. New York: Springer-Verlag, 1978, vol. 27.

[10] S.-H. Lee, J. Kim, F. Park, M. Kim, and J. E. Bobrow, "Newton-type algorithms for dynamics-based robot movement optimization," in *IEEE Transactions on Robotics*, vol. 21, 2005, pp. 657-667.

[11] S. Lengagne, N. Ramdani, and P. Fraisse, "Planning and fast re-planning safe motions for humanoid robots," *IEEE Transactions on Robotics*, vol. 27, no. 6, pp. 1095-1106, dec. 2011.

[12] M. Vukobratović and B. Borovac, "Zero-moment point: Thirty five years of its life," *International Journal of Humanoid Robotics*, vol. 1, no. 1, pp. 157-173, 2004.

[13] H. Hirukawa, S. Hattori, K. Harada, S. Kajita, K. Kaneko, F. Kanehiro, K. Fujiwara, and M. Morisawa, "A universal stability criterion of the foot contact of legged robots - adios zmp," in *IEEE International Conference on Robotics and Automation*, may 2006, pp. 1976-1983.

[14] A. Wächter and L. T. Biegler, "On the implementation of a primal-dual interior point filter line search algorithm for large-scale nonlinear programming," *Mathematical Programming*, vol. 106, pp. 22-57, 2006.