/**

\page AutoBalancer

\section introduction Overview

This component is generate walking pattern and control Center Of
Gravity (COG) for legged robots without sensor feedback.

\subsection cog COG control

This component modifies input joint angles to track reference COG position. 
This component receives reference joint angles from "qRef" inport. 
Then it solves Inverse Kinematics (or something) to track reference COG position, 
obtains jonint angles, and outputs the joint angles as "q" outport.   

In this calculation, this component tracks reference XY COG position, 
reference Z base position, and reference base orientation.  

\subsection mode 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::AutoBalancerService::startAutoBalancer() are called in IDLE mode,  it switches to 
controlling mode ("MODE_ABC"), in which it modifies joint angles. 
When OpenHRP::AutoBalancerService::stopAutoBalancer() are called in controlling mode, it switches to idle mode. 

\subsection eef Controlled end-effectors  

User can specify which end-effector is controlled 
by the argument of OpenHRP::AutoBalancerService::startAutoBalancer(). 
When OpenHRP::AutoBalancerService::startAutoBalancer() 
are called with ["rleg", "lleg"], 
it controlles reference end-effector 
position and orientation for "rleg" and "lleg".
When OpenHRP::AutoBalancerService::startAutoBalancer() are called with
["rleg", "lleg", "rarm", "larm"], 
it controlles reference end-effector for "rleg", "lleg", "rarm", "larm". 
In this case, the robot seems to keep both feet and hands position and orientation. 

\subsection wpg Walking pattern generation  

This component has also the feature as walking pattern generation, named as GaitGenerator. 
When OpenHRP::AutoBalancerService::goPos(), OpenHRP::AutoBalancerService::goVelocity(), OpenHRP::AutoBalancerService::setFootSteps() are called, 
this component starts to use reference XY COG position from "GaitGenerator". 
For OpenHRP::AutoBalancerService::goPos() and OpenHRP::AutoBalancerService::setFootSteps(), 
this component stops to use it after completing walking command. 
After OpenHRP::AutoBalancerService::goVelocity() is called, this component continuously generates walking pattern. 
When OpenHRP::AutoBalancerService::goStop() are called in this case, 
this completing stops to use it after completing walking command.

\subsection pcexample Preview Controller Example
Preview controller example, in which trajectories are displayed on graphs
(COG, ZMP, ... etc). 

@code
testPreviewController # Non-ROS environemnt
rosrun hrpsys testPreviewController # ROS environemnt
@endcode

\subsection ggexample GaitGenerator Example
GaitGenerator example, in which trajectories are displayed on graphs 
(COG, ZMP, foot trajectories ... etc).

Users can change test type (fwd-walking example, rotation-walking example, stair-walking, toe-heel example, ... etc).

Users can also change GaitGenerator parameters (step time, step height, orbit type, ... etc).

@code
testGaitGenerator [test-type-option] [gg-parameters-options] # Non ROS environment
rosrun hrpsys testGaitGenerator [test-type-option] [gg-parameters-options] # ROS environment
@endcode
[test-type-option] is test type and [gg-parameters-options] are GaitGeneratorParameters.
To learn test type, please execute testGaitGenerator without arguments.

\subsection moreabcggdocument More Documentation, Figures, and Explanation for AutoBalancer and GaitGenerator
<A HREF="https://github.com/fkanehiro/hrpsys-base/raw/master/rtc/AutoBalancer/hrpsys_AutoBalancer_GaitGenerator_memo.pptx">AutoBalancerGaitGeneratorDocumentationSlide</a>

<table>
<tr><th>implementation_id</th><td>AutoBalancer</td></tr>
<tr><th>category</th><td>example</td></tr>
</table>

\subsection toejoint About Toe Joints
AutoBalancer can support toe joints.
Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link. 
Without toe joints, "end-effector link == force-sensor link" is assumed.
With toe joints, "end-effector link != force-sensor link" is assumed.

\subsection handfunc Hands and arms functionality
In AutoBalancer, hands and arms has several functionality for two legs and two arms robot.
1. No ik mode. If startAutoBalancer with ["rleg", "lleg"], arms IK are not solved while MODE_ABC.

2. IK mode without fix. If startAutoBalancer with ["rleg", "lleg", "rarm", "larm"] and leg_names is ["rleg", "lleg"] and is_hand_fix_mode is false, arms IK are solved while MODE_ABC and 
Hands target pos are determined according to COG velocity during walking. So, hands seems to follow upper body swinging and total behavior looks like No IK mode.

3. IK mode with fix. If startAutoBalancer with ["rleg", "lleg", "rarm", "larm"] and leg_names is ["rleg", "lleg"] and is_hand_fix_mode is true, arms IK are solved while MODE_ABC.
Hands target pos Y in foot coords are not change during walking and pos X in foot coords is determined according to COG velocity X. 
So, hands seems to be fixed and follow foot projected coordinated. 

4. Four legged walking mode. If startAutoBalancer with ["rleg", "lleg", "rarm", "larm"] and leg_names is ["rleg", "lleg", "rarm", "larm"],
arms IK are solved while MODE_ABC and hand coords are determined by GaitGenerator as walking target.

\section dataports Data Ports

\subsection inports Input Ports

<table>
<tr><th>port name</th><th>data type</th><th>unit</th><th>description</th></tr>
<tr><td>qRef</td><td>RTC::TimedDoubleSeq</td><td>[rad]</td><td>Reference joint angles</td></tr>
<tr><td>zmpIn</td><td>RTC::TimedPoint3D</td><td>[m]</td><td>Input 
ZMP in base frame</td></tr>
<tr><td>basePosIn</td><td>RTC::TimedPoint3D</td><td>[m]</td><td>Input 
base position</td></tr>
<tr><td>baseRpyIn</td><td>RTC::TimedOrientation3D</td><td>[rad]</td><td>Input 
base orientation (RPY)</td></tr>
</table>

\subsection outports Output Ports

<table>
<tr><th>port name</th><th>data type</th><th>unit</th><th>description</th></tr>
<tr><td>q</td><td>RTC::TimedDoubleSeq</td><td>[rad]</td><td>Output
reference joint angles</td></tr>
<tr><td>zmpOut</td><td>RTC::TimedPoint3D</td><td>[m]</td><td>Output 
ZMP in base frame</td></tr>
<tr><td>basePosOut</td><td>RTC::TimedPoint3D</td><td>[m]</td><td>Output 
base position</td></tr>
<tr><td>baseRpyOut</td><td>RTC::TimedOrientation3D</td><td>[rad]</td><td>Output 
base orientation (RPY)</td></tr>
<tr><td>baseTformOut</td><td>RTC::TimedDoubleSeq</td><td>[m], [rad]</td><td>Output 
base position and orientation (RPY)</td></tr>
<tr><td>accRef</td><td>RTC::TimedAcceleration3D</td><td>[m/s^2]</td><td>Output 
attitude sensor's acceleration</td></tr>
<tr><td>contactStates</td><td>RTC::TimedBooleanSeq</td><td>NA</td><td>
Reference end-effector contact states (True=contact, False=no-contact)</td></tr>
<tr><td>controlSwingSupportTime</td><td>RTC::TimedDouble</td><td>[s]</td><td>
Time of swing phase</td></tr>
</table>

\section serviceports Service Ports

\subsection provider Service Providers

<table>
<tr><th>port name</th><th>interface name</th><th>service type</th><th>IDL</th><th>description</th></tr>
<tr><td>AutoBalancerService</td><td>service0</td><td>AutoBalancerService</td><td>\ref OpenHRP::AutoBalancerService</td><td>AutoBalancerService</td></tr>
</table>

\subsection consumer Service Consumers

N/A

\section configuration Configuration Variables

<table>
<tr><th>name</th><th>type</th><th>unit</th><th>default
value</th><th>description</th></tr>
<tr><td>debugLevel</td><td>int</td><td></td><td>0</td><td>debug level</td></tr>
</table>

\section conf Configuration File

N/A

 */
