Mehdi Benallegue

Rafael Cisneros-Limón

(AIST)

• HRP-5P

– AIST’s humanoid robot development history

– Development of HRP-5P

– Application to large scale construction

• Whole-body motion framework

– Perception based locomotion system

– Multi-contact motion generation

– Multi-contact motion control

• Summary and future work

Development and main application

• Height: 1.830 m

• Weight: 101 kg

• 37 DoF

– Neck: 2 DoF

– Waist: 3 DoF

– Arms: 9 DoF x 2

– Hands: 1 DoF x 2

– Legs: 6 DoF x 2

• Special characteristics of the mechanical design:– High power joints

– Wide range joints

– Suitable joint configuration

Kaneko, K., et al. “Humanoid Robot HRP-5P: an Electrically Actuated Humanoid Robot with High Power and

Wide Range Joints”, IEEE Robotics and Automation Letters, vol. 4, no. 2, 2019

Presented at ICRA 2019 MoC1-07.6 16:15〜17:30, May 20, 2019

• Oil lubrication system for the Harmonic Drive Gears

– Increase input rotational speed

• Multi-motor drive system (double or triple)

– Multiple motors connected to the

same synchronous belt

– Easier to mount in links

with restricted shapes

• Air cooling system

• Double speed and torque

of HRP-2Kai

• Intersecting

joint axes

• Link design

• Component

placement

HRP-2KAI

HRP-5P

vs

Right ankle joint (roll) motion range

• Unlikely to have a singular configuration

– Offsets to avoid axes to become colinear

• Scapula joint high reachability

• These special characteristics allowed to achieve human-like motions

– Stretching and high-demanding poses

KVH Industries: 1750 IMU

Carnegie Robotics: Multisense-SL

Orbbec: Astra

Locomotion system,

motion generation and control

1. Localization• Odometry fusion based on a

particle filter • Adjustment of waist height

based on FK and ground height

2. Environmental memorization• Occupancy grid map for

collision avoidance• Height field for landing state

and waist height estimation

3. Footstep management• Adjustment of globally planned

footsteps

15

Kumagai, I., et al. “Perception Based Locomotion System for a Humanoid Robot with Adaptive Footstep Compensation under Task Constraints”,

IEEE/RSJ International Conference on Inteligent Robots and Systems, 2018

• Fast computation method for 3D multi-contact

motion generation (60 s)

• Key ideas:

– Force distribution ratio

formulation similar to the inverted pendulum

– Split the CoM trajectory into:• Ideal trajectory (long term) with analytical solution

• Its tracking (short term) using state feedback by pole assignment

• It enables to update the locomotion parameters at any time

Morisawa, M., et al. “Online 3D CoM Trajectory Generation for Multi-Contact Locomotion Synchronizing Contact”,

IEEE-RAS International Conference on Humanoid Robots, 2018

• Multi-contact CoM dynamics

3

3[ ]

tG

c c

G G tr

m m

m m

I 0 g 0Jpf n

p I p g JJL

11

( )

kyG

G G zi xiki GG xi zii

g zx x p

m g zz p

vs

3 int( )r k t t G t c

σ J I J J f L J n

( )

yGG G zmp

G zmp G

Lg zx x x

z z m g z

“Support height”“Horizontal support

position”

1( )T T

t t t t

J WJ J WJ

1

T

t L

J α α

mg

gp

gmP

• Linear Time-Variant System high computation cost

• Alternative:

– Ideal trajectory + tracking (due to changes in contact position / timing)

0

0.2

0.4

0.6

0.8

0 1 2 3 4 5 6 7 8

Current time: 7sPreview window [5.4 : 8s]

time [s]

, ,t

0

0.2

0.4

0.6

0.8

0 1 2 3 4 5 6 7 8

time [s]Posi

tion

(sag

itta

l) [

m]

Current time: 1sPreview window [0 : 2.6s]

, ,t

0

0.2

0.4

0.6

0.8

time [s]0 1 2 3 4 5 6 7 8

Current time: 3sPreview window [1.4 : 4.6s]

19

Allows

• Dynamics-aware feed forward torque

• Straightforward passive compliance (higher robustness to environment errors)

But

• Inaccurate without torque feedback.

• Sensitive to modeling errors (lower robustness)

20

• Landau et al (1989). Applications of the passive systems approach to the stability analysis of adaptive controllers for robot

manipulators. International Journal of Adaptive Control and Signal Processing, 3(1), 23-38.

• Slotine et al (1987). On the adaptive control of robot manipulators. The international journal of robotics research, 6(3), 49-59.

𝜏 = 𝑀 𝑞 𝑞 + 𝐶 𝑞, 𝑞 𝑞 + 𝐺 𝑞 −

𝑖

𝐽𝑖𝑇 𝑞 𝐹𝑖

Computed torque

𝜏𝑐 = 𝑀 𝑞 𝑞𝑟 + 𝐶 𝑞, 𝑞 𝑞 + 𝐺 𝑞 −

𝑖

𝐽𝑖𝑇 𝑞 𝐹𝑖

𝑟

Passivity-based torque control

𝜏𝑝 = 𝑀 𝑞 𝑞𝑟 + 𝐶 𝑞, 𝑞 𝑞𝑟 + 𝐺 𝑞 −

𝑖

𝐽𝑖𝑇 𝑞 𝐹𝑖

𝑑

Guarantees exponential convergence of 𝑞𝑟 − 𝑞 𝑡𝑜 0.

With 𝐶 𝑞, 𝑞 + 𝐶𝑇 𝑞, 𝑞 = 𝑀(𝑞, 𝑞)

With 𝑞𝑟 = 𝑞𝑟 𝑑𝑡

+𝐾( 𝑞𝑟 − 𝑞)

21

𝜏𝑝 = 𝑀 𝑞 𝑞𝑟 + 𝐶 𝑞, 𝑞 𝑞𝑟 + 𝐺 𝑞 −

𝑖

𝐽𝑖𝑇 𝑞 𝐹𝑖

𝑟 + 𝐾 𝑞𝑟 − 𝑞

= 𝑀 𝑞 𝑞𝑟 + 𝐶 𝑞, 𝑞 𝑞 + 𝐺 𝑞 −

𝑖

𝐽𝑖𝑇 𝑞 𝐹𝑖

𝑟 + (𝐾 + 𝐶) 𝑞𝑟 − 𝑞

= 𝜏𝑟 + (𝐾 + 𝐶) 𝑞𝑟 − 𝑞

Integral termComputed torque

22

𝜏𝑝 = 𝜏𝑟 + (𝐾 + 𝐶) 𝑞𝑟 − 𝑞Pros

• Improves robustness to low frequency disturbances

• most of friction

• biases in mass distribution

• Gets rid of static error

• Allows high frequency compliance with less drawbacks.

• Suitable for adaptive control

Cons

• Wind up in case of unmodeled constraints

• How to add integral term to non actuated DoF?

23

𝜏𝑝 = 𝜏𝑟 + (𝐾 + 𝐶) 𝑞𝑟 − 𝑞

• How to add integral term to non actuated DoF?

If

𝜏𝑏𝑟 = 𝑀𝑏 𝑞 𝑞𝑟 + 𝐶𝑏 𝑞, 𝑞 𝑞 + 𝐺𝑏 𝑞 −

𝑖

𝐽𝑏𝑖𝑇 𝑞 𝐹𝑖

𝑟 = 0

then

𝜏𝑏𝑝= 𝐾𝑏 + 𝐶𝑏 𝑞𝑟 − 𝑞 ≠ 0 → Not feasible

That means that

if we want 𝜏𝑏𝑝= 0 we must have 𝜏𝑏

𝑟 ≠ 0 → not feasible! → 𝑞𝑟 not feasible either!

24

𝜏𝑝 = 𝜏𝑟 + (𝐾 + 𝐶) 𝑞𝑟 − 𝑞

• How to add integral term to non actuated DoF?

With a QP, we can compute the optimal, generally

non feasible, accelerations that can be tracked

robustly with feasible torques.

25

𝐿 = (𝐾 + 𝐶)𝛼 = 𝑞

Cisneros, R., Benallegue, M., et al. “Robust humanoid control using a QP solver with integral gains”,

IEEE/RSJ International Conference on Inteligent Robots and Systems, 2018

Task 1

Task k

1

1

1

A

b

W

QP

Motion

Solver

A

b

W

refα

iA

ib

iWConstraints

k

k

k

A

b

W

• Tasks:

– Posture Task (joint angles)

– Position Task (any link)

– Orientation Task (any link)

– CoM Task

– Wrench Task

(any contacting link)

– Admittance Task

(any contacting link with F/T sensor)

• Constraints:

– Underactuation constraint (floating base dynamics)

– Torque limit constraint

– Joint limit constraint (simultaneous position and velocity)

– Friction constraint (lumped forces within friction cone approximation)

– Surface frame constraint (equivalent to position/orientation task but as a constraint)

Cf

1β

2β3β

4β

• Simulink Simscape Multibody

(dynamics simulation)

• No torque feedback

• Uncertainties:

– Realistic joint frictions

– Random mass distribution error

in the model assumed by the controller

• Two cases:

– Inverse dynamics

(without integral term)

– Passivity-based integral term

(integral term considered in QP constraints) Inverse

Dynamics

Passivity-Based

Integral Term

• AIST dynamics simulator of Choreonoid

+ QP solver of the mc_rtc library

• Evaluation of multi-contact motions

• Evaluation of hybrid control

(force + position)

– No balance control considered yet

• Development of HRP-5P thought for

– Demanding human-like motions

– Dexterous manipulation

• Perception locomotion system using odometry and environmental memorization

• Fast multi-contact motion generation

• Multi-objective motion control using a passivity-based integral term

– No torque feedback

• Future work:

– HRP-5P’s position control torque control

– Balance control task

– Multi-contact motion generation and control on the real HRP-5P