Overview

This component is maintain full-body balance based on sensor feedback.

Feature

This component modifies input joint angles to control COG, ZMP, and so on. This component receives reference joint angles from "qRef" inport. Then it solves Inverse Kinematics (or something) based on sensor feedback, obtains jonint angles, and outputs the joint angles as "q" outport.

Mode

This component has modes and behaves as Finite State Machine. By default, this component is idle mode ("MODE_IDLE"), in which it do not modify input joint angles. When OpenHRP::StabilizerService::startStabilizer() are called in idle mode, it switches to controlling mode ("MODE_ST"), in which it modifies joint angles. When OpenHRP::StabilizerService::stopStabilizer() are called in controlling mode, it switches to idle mode. When this component is controlling mode and the robot is put off to the air, it switches to air mode ("MODE_AIR"), which is similar to idle mode. When this component was air mode and the robot is put on the ground, it switches to controlling mode.

ZMPDistributor Example

ZMPDistributor example, in which distributed force and moment are displayed on graphs (COP, Ref-force, Ref-moment, ... etc).

testZMPDistributor # Non ROS environment
rosrun hrpsys testZMPDistributor # ROS environment

implementation_id	Stabilizer
category	example

Stabilizer algorithm

Stabilizer algorithm is based on the following papers:

• EEFM (partially, EEFMQP, EEFMQPCOP):
  [1] Shuuji Kajita, Mitsuharu Morisawa, Kanako Miura, Shin’ichiro Nakaoka,Kensuke Harada, Kenji Kaneko, Fumio Kanehiro and Kazuhito Yokoi "Biped Walking Stabilization Based on Linear Inverted Pendulum Tracking", The 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems, p. 4489-4496, 2010
• TPCC:
  [2] 長坂,稲葉,井上:"体幹位置コンプライアンスを用いた人間型ロボットの歩行安定化",第 17 回日本ロボット学会学術講演会予稿集, pp.1193–1194, 1999.
  [3] Y. Choi, D. Kim and B.-J. You: "On the Walking Control for Humanoid Robot based on the Kinematic Resolution of CoM Jacobian with Embedded Motion," Proc. of 2006 IEEE Inter. Conf. on Robotics and Automation, pp.2655–2660, 2006.

Data Ports

Input Ports

port name	data type	unit	description
qCurrent	RTC::TimedDoubleSeq	[rad]	Actual joint angles
qRef	RTC::TimedDoubleSeq	[rad]	Reference joint angles
rpy	RTC::TimedOrientation3D	[rad]	Actual attitude sensor's Roll-Pitch-Yaw angle
forceL	RTC::TimedDoubleSeq	[N],[Nm]	6D wrench vector for actual force and moment for left leg
forceR	RTC::TimedDoubleSeq	[N],[Nm]	6D wrench vector for actual force and moment for right leg
zmpRef	RTC::TimedPoint3D	[m]	Reference ZMP in base frame
basePosIn	RTC::TimedPoint3D	[m]	Reference base position
baseRpyIn	RTC::TimedOrientation3D	[rad]	Reference base orientation (Roll-Pitch-Yaw)
contactStates	RTC::TimedBooleanSeq	NA	Reference end-effector contact states (True=contact, False=no-contact)
controlSwingSupportTime	RTC::TimedDouble	[s]	Time of swing phase

Output Ports

port name	data type	unit	description
q	RTC::TimedDoubleSeq	[rad]	Output reference joint angles
zmp	RTC::TimedPoint3D	[m]	Estimated actual ZMP in base frame

Service Ports

Service Providers

port name	interface name	service type	IDL	description
StabilizerService	service0	StabilizerService	OpenHRP::StabilizerService	StabilizerService

Service Consumers

N/A

Configuration Variables

name	type	unit	default value	description
debugLevel	int		0	debug level

Configuration File

N/A