Robust Vertical Ladder Climbing and Transitioning between Ladder and Catwalk for Humanoid Robots

Transcription

1 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Congress Center Hamburg Sept 8 - Oct,. Hamburg, Germany Robust Vertical Ladder Climbing and Transitioning between Ladder and Catwalk for Humanoid Robots Masao Kanazawa, Shunichi Nozawa, Yohei Kakiuchi, Yoshiki Kanemoto, Mitsuhide Kuroda, Kei Okada, Masayuki Inaba, Takahide Yoshiike Abstract This paper presents a novel control method to stabilize the whole-body motion of humanoid robots when climbing vertical ladders and transitioning between ladders and catwalks. In such environments, the body of the robot tends to incline and rotate because of the slippery surfaces. The inclination and rotation may cause the robot to fail to grasp and thus collide with the rungs. The proposed method modifies the subsequent contact position in real time based on the error of the current robot posture estimated with inertial measurement units (IMUs) and actual joint angles. This paper also presents a method of generating motion by minimizing the contact wrench. This method satisfies hardware limitations, such as collision avoidance, joint torque limits, and joint limits. Applying these methods to a humanoid robot, we realize the robust climbing and descending of multiple rungs of a vertical ladder and bidirectional transitioning from ladders to catwalks. I. INTRODUCTION Recently, humanoid robots have been expected to travel in environments designed for humans during the inspection or maintenance of social infrastructures, such as factories and power plants. In particular, climbing and descending ladders and transitioning between ladders and catwalks are necessary for activities in such places. It is difficult for a humanoid robot to climb a ladder because the motion of the robot must simultaneously satisfy various constraints, including joint angle limit, joint torque limit, balancing, avoiding collisions, and compensating for friction [], []. Noda et al. performed an experiment on stepladder climbing based on iterative motion optimization at key poses [3]. In the DARPA Robotics Challenge, several teams have also presented stepladder-climbing robots [4], []. However, climbing vertical ladders is more challenging than climbing stepladders because the weak frictional force makes the robot likely to slip on the rung and the horizontal component of the center of gravity (COG) position always exceed the support polygon made by its feet. In particular, an actual motion tends to deviate from a designed motion when gripping with one hand. This deviation causes the robot to fail to grasp the next rung and eventually results in the robot falling from the ladder. Vaillant et al. [6] experimented with a robot climbing a vertical ladder. In their experiment, the gripper position was adjusted by an operator located behind the robot because the robot lacked a firm grasp. An error between the actual and designed motions caused by Fundamental Research Center, Honda Research & Development, 8- Honcho, Wako-shi, Saitama, Japan. Department of Mechano-Infomatics, The University of Tokyo, 7-3- Hongo, Bunkyo-ku, Tokyo, Japan Fig.. Vertical ladder climbing (left) and transitioning between a ladder and a catwalk (right) mechanical compliance is inherent and unavoidable because of translational and rotational errors, including joint position error, compliance, or insufficient grasping force to climb ladders. Therefore, robots must be able to compensate for this error to continue climbing. Similar problems exist when the robot makes transition between a ladder and a catwalk. When making this transition, the robot tends to rotate around the edge of the support polygon if the hands cannot provide the appropriate contact force and moment to maintain its balance, because the horizontal components of the COG position usually exceed the support polygon. In this case, the robot s feet may slip on the rung, causing the robot to collide with or fall from the ladder. Because of these problems, humanoid hardware capable of transitioning between a ladder and a catwalk has not yet been developed. In this paper, we propose a novel control method to modify the subsequent contact position according to the current estimated robot posture in real time. We realize robust multiplerung vertical ladder climbing and bidirectional transitioning from a ladder to a catwalk by humanoid robot hardware using the proposed control method and a complementary motion generation method. The proposed motion generation method satisfies the various constraints described above. The following section gives an overview of the proposed system. The motion generation method is explained in Sections III and IV, and the stabilizing control is explained in Section V. Section VI presents the experimental results //$3. IEEE

2 II. OVERVIEW OF PROPOSED SYSTEM Our system consists of two primary modules that perform offline motion generation and real-time motion control, as shown in Fig.. The motion generation module outputs motion that satisfies the joint and torque constraints and avoids both collisions with the environment and self-collisions under the constraints of the frictional forces between each rung of the ladder and the robot s feet. Here, the motion of the robot is defined as the trajectories of the joints, the trajectory of the base link, and the contact wrenches, which are the wrenches at the contact points. Using this definition, the robot can execute the designed motion. The motion control module directs the robot to follow a motion generated offline in real time. In the motion generation module, the motion is generated in the following sequence. ) Target contact point generation: Target contact points are predefined in the environmental geometry model. A sequence of contact points corresponding to a robot crawl climbing a ladder is shown in Fig.3. ) Trajectory interpolation: End-effector trajectories are generated by interpolating the target contact points and waypoints. Waypoints are defined for the robot s hands and feet based on the direction of approach. 3) Whole-body joints and base link trajectories generation: Based on the target contact points, the key-frame target COG positions are determined, and the target COG trajectories are then obtained by interpolation. In Section III, whole-body joints and base link trajectories generation is explained in detail. 4) Contact wrench distribution by optimization: All inertial forces are calculated from inverse dynamics based on the joints and the base link trajectories. The contact wrenches of the end-effectors are then obtained from optimization considering contact constraints, such as a friction cone. In Section IV, contact wrench trajectory generation is explained in detail. ) Stability control: The robot essentially follows the designed trajectories of the base posture, joint angles, and contact wrench. The designed joint angles that are sent to the robot are modified in real time by the stability control based on sensors, such as force/torque sensors and inertial measurement units (IMUs). III. WHOLE-BODY JOINTS AND BASE LINK TRAJECTORIES GENERATION To generate motion that satisfies various constraints, such as joint angle and joint velocity limits, we formulate the problem as a quadratic programming (QP) problem with inequality constraints. The objective function and the inequality constraints of the QP problems are min q q T W a q + k (ẋ k J k q) T W k (ẋ k J k q) () s.t. q q q + () ẋ col J col q, (3) where q R 6+N D.O.F. is the generalized velocity vector and includes the joint, base link, and angular velocities; ẋ k Fig.. Fig. 3. Gait Parameters (Number of steps; gait pattern, stab.: modified reference by stability control e.g., crawl, pace, trot) Motion generation Target contact point generation Contact points Trajectory interpolation Stability control End-effector Momentum Body posture trajectory Whole-body trajectory generation Base posture reference Joint angle reference Contact wrench distribution Base posture reference Joint angle reference Contact wrench reference Joint angle stab. Hardware Motion control Overview of system for motion generation and control Sequence of target contact points (red) on ladder is a generalized velocity vector in the task space; J k is a Jacobian; W a is a weight matrix for the joint velocity; and W k is a weight matrix for the tasks. The second term of Equation () describes the minimization of the errors of the equality constraints in the task space. The definitions of these constraints are listed in the following subsections. Equation () corresponds to the joint and joint velocity constraints, and Equation (3) corresponds to self-collision and robot environment collision avoidance constraints [7]. A. End-effector constraints Target end-effector trajectories should be tracked accurately to avoid collisions with environmental objects, such as ladder rungs. The velocities of the end-effectors must satisfy the constraint ẋ E = J E q, (4) where ẋ E is the target velocity vector of the end-effectors and J E is the Jacobian of the end-effector velocities. When using velocity constraints, the end-effector positions contain numerical errors. Therefore, the target velocity vector is generated by using proportional control to follow the desired end-effector trajectories. B. COG velocity constraints To satisfy the kinematic constraints, we use an additional constraint to follow the COG trajectory. The COG trajectory 3

3 Fig. 4. The frame of the robot (red: x-axis, green: y-axis, blue: z-axis) is used instead of the base link trajectory to ensure the total inertial force is smooth. To generate the COG trajectory, the COG sequence at key poses is calculated by averaging the positions of all contact points and then adding a predefined offset. The COG is then interpolated with a minimum jerk model. Because of the numerical error, proportional control is also used to follow the COG trajectory. The COG velocities must satisfy the constraint [8], [9]. where J g is the COG Jacobian. C. Base link orientation constraints ẋ g = J g q, () To satisfy the joint limits, the base link orientation is constrained around the x- and y-axes by the selection matrix S x,y, which is given by S x,y ẋ r = S x,y J r q (6) [ ] S x,y =, (7) where J r is the Jacobian for the base link angular velocity. The frame of robot is shown in Fig.4. Because we employ q R 6+N D.O.F., as mentioned in (), we can formulate the base link angular velocity like the joint space velocity. We assume that the base link is set to the root of the link chains, which is a pelvis link in the case of a typical humanoid robot. D. Angular momentum constraints For the transition between a ladder and a catwalk, the rotational error of the base link around the yaw axis is considered critical because it results in the robot s failure to grasp. To maintain an angular momentum of zero about the yaw axis, the following constraint to the selection matrix S z must be satisfied: S z ẋ l = S z J l q, (8) where J l is the angular momentum Jacobian [8], [9]. The inverse kinematics are calculated using this method. The calculated joint and base link velocities satisfy the joint angle and joint angle velocity constraints while tracking the end-effector position and orientation, COG position, base link orientation around the roll and pitch axes, and angular momentum around the yaw axis. The joint angles and the base link position and orientation are calculated by integrating the velocity trajectory. IV. CONTACT WRENCH DISTRIBUTION A. Inverse dynamics calculation of whole-body inertial wrench The whole-body inertial wrench is calculated using inverse dynamics as [] [ ] [ ] ] [ ] M M = b + τ M M [ qb θ b Σ k [ J T k, J T k, J T k, J T k, ] [ fk n k ], (9) where M ii is the generalized inertia matrix, b i is the generalized bias force (gravitational force and Coriolis forces), J k,ii is the Jacobian of the contact point for kth limb, f k, n k are the contact forces and moments, respectively, q B is acceleration of base link, θ are joint angles, τ are joint torques. Assuming that the contact wrenches of all limbs are zero, the whole-body inertial wrench w B can be obtained as [ ] [ ] [ ] wb M M τ = b +, () M M ] [ qb θ where τ is the modified joint torques. B. Distribution of whole-body inertial wrench by quadratic programming In Equation (), we assume that the contact wrenches of all feet and hands are zero but instead obtain w B, which is the whole-body inertial wrench applied to the base link. In practice, the whole-body inertial wrench should be balanced with the contact wrenches, and the total wrench applied to the base link should be zero. While determining the distribution of the whole-body inertial wrench, constraints of the contact condition and joint torque limits must be satisfied. We formulate this constrained inverse dynamics problem as a QP problem as follows: min w all wall T W wrw all + τ T W trq τ () s.t. G all w all = w B () b C all w all (3) τ min τ τ max, (4) where W wr and W trq are the weight matrices of the wrenches and joint torques, respectively; τ min and τ max are the minimum and maximum torque limits, respectively; and w all R 6N L is the contact wrenches of all limbs with N L representing the number of contact limbs. w all = [ wc,, T, wc,i T,, wt c,n L ] T () Equation () shows the distribution of the whole-body inertial wrench. G all R 6 6N L shows the equality matrix of the contact wrench: [ ]... E3... G all w all = w... p c,i E 3... all, (6) where p c,i is the outer product matrix of p c,i, which is the position of the ith contact point. 4

4 Considering wrenches at contact points (p c,z = ), we obtain p cop,x = p c,x f c,z n c,y f c,z () p cop,y = p c,y f c,z + n c,x f c,z. (3) Fig.. Frame of hand (left) and foot (right) Equation (3) shows the constraints of the contact conditions, which are C all w all = diag(c c,i R 6,c,i )w all, (7) where R 6,c,i R 6 6 is a transformation matrix from contact coordinates to the fixed environment coordinates. [ ] Rc,i R 6,c,i = (8) R c,i The contact constraint C c,i is described in the following section. Note that the decoupling of the motion generation into Equations () and () does not guarantee that the obtained motion is feasible. For example, when calculating Equation () after Equation (), we may not find the optimal solution of Equation (). To confront this problem in our approach, we use a heuristic rule to modify the key pose COG sequence such that it satisfies the dynamics constraints. C. Contact conditions When calculating the contact constraint C c,i, we employ local contact coordinates to determine the contact constraint matrices at each contact point. We consider plane and line contacts at the foot and a gripper contact at the hand, as shown in Fig.. For the plane contact, we define the z-axis as the component normal to the contact plane and the x- and y-axes as the components tangential to the contact plane. We consider four contact conditions as follows. The wrench expressed in the contact coordinates is w c = [f T c n T c ] T, f c = [f c,x f c,y f c,z ] T, and n c = [n c,x n c,y n c,z ] T, where the moment n c is defined with respect to the contact point p c. Unilateral constraint: Frictional cone: f c,z. (9) f c,[x,y] µ t f c,z. () Rotational friction around z-axis: n c,z µ r f c,z, () where µ t and µ r are the translational and rotational friction coefficients, respectively. Center of pressure (COP) constraint The condition that the COP is inside a rectangular area with principal axes aligned with the coordinates of the contact point is described as l min,[x,y] p cop,[x,y] l max,[x,y]. (4) We model the contact between the feet and the rung as a line contact. For the line contact, we set several COP margins in (4) to zero because the COP is limited on the line. For example, when the line is oriented along the y-axis, we set l min,x = l max,x =. For the plane or line contact of the foot, we can calculate the following contact matrix: C c,i = [ C T uni,z, CT tfic,x, CT tfic,y, CT rfic,z, CT cop,x, C T cop,y ] T i, () where C uni,z, C tfic,[x,y], C rfic,z, and C cop,[x,y] are calculated from Equations (9), (), (), and (4), respectively. For the gripper contact of the hand, we approximately use the following contact matrix instead of detailed nonlinear constraints: C c,i = [ C T uni,z, CT tfic,y, CT rfic,y, CT cop,x V. STABILITY CONTROL ] T i (6) If the robot can accurately trace the motion generated using the method shown in Sections III and IV, the robot would be able to climb and descend a ladder. However, a humanoid robot often deviates from designed motion because of mechanical compliance, including joint position error or lack of a firm grasp. The rotation error should be considered while the robot ascends a vertical ladder. We have proposed a novel control algorithm that decreases the rotation error in real time by modifying the position of the end-effector according to the estimated robot posture and actual joint angles. Fig.6 provides an overview of the stability control algorithm. The stability controller consists of three components: a limb compliance controller, a robot posture estimator, and a real-time footstep position controller. A. Limb compliance controller The limb compliance controller regulates the contact wrench for each limb. This control suppresses excessive joint torque caused by internal forces among the limbs. To reduce high-frequency inputs to the force/torque sensors, the soles and grippers of the robot are equipped with rubber bushes.

5 B. Robot posture estimator In this section, we propose a robot posture estimator that can estimate the positions and orientations of the contact points and the base link. Rotational and translational errors occur due to mechanical compliance, joint position error, and slippage of the feet. The purpose of the robot posture estimator is to estimate these rotational and translational errors. The overall flow of the estimation method is shown in Fig.7, and the parameters of the estimator are listed in Table I. First, the orientation of the base link Rbase estm is estimated by inertial navigation with an accelerometer and a gyroscope. Next, the positions and orientations of the end-effectors are calculated by forward kinematics using actual joint angles measured by encoders, the orientation of the base link estimated by inertial navigation, and the translation of the base link previously estimated by the robot posture estimator: (x estm sup [k], R estm [k], x estm [k], R estm [k]) sup swg swg = F orward Kinematics(x estm base [k ], R estm base [k], θ act [k]). (7) Next, the estimator calculates the deflections of the elastic mechanical elements, such as rubber brushes, attached to the end-effectors. The contact points are estimated as x estm sup [k] = x estm sup [k] K f f sup [k] (8) R estm sup [k] = exp( K τ τ sup [k])rsup estm [k], (9) where K f and K τ are respectively the force and torque gains of the mechanical compliance of the end-effectors. The mechanical compliance is identified by measurement or structural calculation. The torque element of the mechanical compliance exp(k τ τ sup [k]) is represented with an exponential map []. To estimate the translation of the base link, we assume that the estimated contact points are close to the reference contact points. The estimator minimizes the sum of the estimation errors of all contact points. The estimation errors of the contact points can be calculated as the differences between the estimated contact points and the target contact points specified by the reference trajectory. The minimization is asymptotically achieved by subtracting the average errors of the contact points multiplied by feedback gain from the previous estimation of the translation of the base link, as x estm base [k] = x estm base [k ] K base n n i= (x estm sup,i [k] x ref sup,i [k]). (3) The positions and orientations of the base link and the endeffectors are estimated using this estimator. With appropriate feedback gain, discontinuities while changing contacts are suppressed. C. Footstep position controller The footstep position controller modifies the trajectories of the end-effectors of the limbs using the estimated contact Contact wrench reference Body posture reference Joint angle reference Whole robot trajectory Symbol ωgyro sens a sens gyro x estm base, Restm base x ref base, Rref base x sup/swg R sup/swg θ act f sup τ sup K base Joint angle Angular velocity Acceleration Robot posture estimator Footstep position controller Fig. 6. Fig. 7. End-effector modified stab. Robot Estimated body posture Estimated end-effector posture Contact wrench Limb compliance controller Inverse kinematics Controller overview Robot posture estimator TABLE I End-effector stab. Joint angle stab. stab.: modified reference by stability control PARAMETERS OF ROBOT POSTURE ESTIMATOR Fig. 8. Definition base link angular velocity from gyroscope base link linear acceleration from accelerometer estimated base link position/orientation reference base link position/orientation support/swing limb end-effector position support/swing limb end-effector orientation actual joint angle contact force of support limb from force sensor contact torque of support limb from torque sensor base link positional error feedback gain Schematic of footstep position controller point positions. The deviation of the limb trajectories causes the problems listed below. 6

6 X [m]. Y [m] Z [m]. Fig. 9. Fig.. Simulated base link position and orientation (green and blue lines almost overlap) E XPERIMENTAL HARDWARE SPECIFICATIONS Degrees of freedom Height Weight Leg Arm Hand Body Head Total.67 m 8 kg VI. E XPERIMENTAL RESULTS A. Experimental hardware We used an experimental robot to validate the algorithm. The configuration and major specifications of the robot are summarized in Fig.9 and Table II. The robot s trunk is equipped with pitch and yaw joints, which enable both bipedal and quadrupedal walking []. Its hands are designed for knuckle-walking and ladder climbing. The pelvis of the robot is assumed to be the base link. Robot fails to grasp or land on the rung. Shank of the robot hits the rung. To solve the first problem, the positions of the endeffectors are modified according to the estimated deviations of the positions of the swing limb end-effectors, as shown in Fig.8. The modifications are filtered by a low-pass filter to prevent oscillations caused by sensor noise: xswg = (xref xestm swg ). + T s swg (3) To solve the second problem, the deviation of the contact point is treated as slippage on the rung of the ladder when the limb is the supporting limb. To maintain the base link at the designed position, the position modification of the endeffector is calculated as xsup = (xref xestm sup ), + T s sup TABLE II Dimension estimated Experimental hardware simulated actual Yaw [deg] Pitch [deg] Roll [deg] designed (3) which means the robot stretches its leg to compensate for the slippage of support limbs. These modifications should be performed in real time to prevent a grasping or landing failure. The modifications should be constrained such that they do not exceed the joint angle limits. With these methods, the rotation and inclination of the robot do not diverge, and collisions against rungs can be avoided. B. Verification of robot posture estimator We verified the robot posture estimator using the rigid body simulator integrated with Open Dynamics Engine [3]. Fig. shows the results of the base link position and orientation estimation. Fig. shows the estimation error of them. The position estimation error is less than 9 mm. The orientation estimation errors are less than. deg. The results of this simulation show that the estimated position and orientation of the base link have sufficiently high accuracy. C. Effects of footstep position controller We evaluated the effects of the footstep position controller in two cases: vertical ladder climbing and transitioning between a ladder and a catwalk. Fig. compares the inclination error of the pitch axis with and without the footstep controller while climbing the vertical ladder. The inclination error is defined as the difference between the estimated and designed body link orientations. Without the footstep position controller, slippage of the foot occurs because the grasping point of the hand works like the pivot of a pendulum. After several steps, the inclination error reaches. deg, and the shank collides with the ladder. In contrast, with the controller, the inclination error does not diverge beyond. deg. The simulation results 7

7 X Y Z position error [mm] Roll Pitch Yaw. Fig.. The effects of the footstep position controller in simulation. The background color represents the swing phase of each limb...6 effector position X [m] rotational error [deg] demonstrate that the controller enables the robust climbing of multiple rungs without grasp failures or collisions. Fig.3 shows the results of simulating a transition from a ladder to a catwalk. We compare the proposed methods with and without the footstep position controller. Without the footstep position controller, the amount of slippage of the left foot is mm after the transitioning, and the shank of the left leg and the ladder may collide during the transition. In contrast, the slippage is. mm with the footstep position controller because the controller appropriately modified the footstep position. VII. C ONCLUSIONS In this paper, we propose a novel control method for the whole-body motion of a humanoid robot while climbing a 4 without footstep control with footstep control 4 6 Fig. 3. Simulated transition from a ladder to a catwalk Above figure: Left leg effector position X, Bottom figure: Left leg effector effector position X error, Right figure: Snapshot of robot when left leg is on the rung (s) and footstep controller is used. D. Real robot experiment. Pitch error [deg]. -. Yaw error [deg] The performance of the proposed controller and motion generator is verified through experiments with the humanoid robot. ) Ladder climbing: Fig. shows snapshots of the robot climbing four rungs of a vertical ladder. The distance between the rungs is.7 m, and the width of each rung is.4 m. The sequence of crawl climbing and descending four rungs takes 8 s. The inclination errors of the roll and pitch axes are less than.9 deg, and the inclination error of the yaw axes is less than. deg as shown in Fig.4. ) Transition between ladder and catwalk: Fig.6 shows snapshots of the humanoid robot transitioning between the ladder and the catwalk. In the experiment, the robot utilized the footstep position controller and could transition stably without collisions. designed. Simulated base link position and orientation estimation error with footstep control. effector position X error [mm] Fig.. Roll error [deg] without footstep control.4 - time[s] Fig. 4. Experimental base inclination error while climbing ladder vertical ladder and transitioning between ladders and catwalks. We have solved the issues of posture error caused by mechanical compliance at the feet and hand grippers, joint position error, and slippage of the feet. The proposed control method modifies the subsequent footstep position in real time according to the estimated current robot posture from IMUs and actual joint angles. 8

8 Fig.. Fig. 6. Snapshots of humanoid robot climbing ladder Snapshots of humanoid robot transitioning between ladder and catwalk We have also developed a motion generation method that minimizes contact wrenches while satisfying kinematic and contact constraints. With these methods, we realized robust continuous vertical ladder climbing and descending and bidirectional transitioning from ladders to catwalks with a human-sized humanoid robot. In this paper, ladder climbing was achieved with a crawl configuration. To design a robot that can climb ladders more rapidly, we plan to employ other gait patterns, such as trotting and pacing, in the future. R EFERENCES [] H. Yoneda, K. Sekiyama, Y. Hasegawa, and T. Fukuda, Vertical ladder climbing motion with posture control for multi-locomotion robot, in Proc. of the 8 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 8, pp [] S. Kim, M. Spenko, S. Trujillo, B. Heyneman, V. Mattoli, and M. R. Cutkosky, Whole body adhesion: hierarchical, directional and distributed control of adhesive forces for a climbing robot, in Proc. of the 7 IEEE International Conference on Robotics and Automation. IEEE, 7, pp [3] S. Noda, M. Murooka, S. Nozawa, Y. Kakiuchi, K. Okada, and M. Inaba, Generating whole-body motion keep away from joint torque, contact force, contact moment limitations enabling steep climbing with a real humanoid robot, in Proc. of the 4 IEEE International Conference on Robotics and Automation. IEEE, 4, pp [4] G. Pratt and J. Manzo, The darpa robotics challenge [competitions], IEEE Robotics & Automation Magazine, vol., no., pp., 3. [] J. Luo, Y. Zhang, K. Hauser, H. A. Park, M. Paldhe, C. Lee, M. Grey, M. Stilman, J. H. Oh, J. Lee, et al., Robust ladder-climbing with a humanoid robot with application to the darpa robotics challenge, in Proc. of the 4 IEEE International Conference on Robotics and Automation. IEEE, 4, pp [6] J. Vaillant, A. Kheddar, H. Audren, F. Keith, S. Brossette, K. Kaneko, M. Morisawa, E. Yoshida, and F. Kanehiro, Vertical ladder climbing by the hrp- humanoid robot, in Proc. of 4th IEEE-RAS International Conference on Humanoid Robots. IEEE, 4, pp [7] F. Kanehiro, M. Morisawa, W. Suleiman, K. Kaneko, and E. Yoshida, Integrating geometric constraints into reactive leg motion generation, in Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems,, pp [8] T. Sugihara and Y. Nakamura, Whole-body cooperative balancing of humanoid robot using cog jacobian, in Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems, vol. 3. IEEE,, pp [9] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi, and H. Hirukawa, Resolved momentum control: Humanoid motion planning based on the linear and angular momentum, in Proc. of the 3 IEEE/RSJ International Conference on Intelligent Robots and Systems, vol.. IEEE, 3, pp [] K. Nagasaka, The whole-body motion generation of humanoid robot using dynamics filter, Ph.D. dissertation,. [] F. S. Grassia, Practical parameterization of rotations using the exponential map, Journal of graphics tools, vol. 3, no. 3, pp. 9 48, 998. [] T. Kamioka, T. Watabe, M. Kanazawa, H. Kaneko, and T. Yoshiike, Dynamic gait transition between bipedal and quadrupedal locomotion, in Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE,. [3] R. Smith, Open dynamics engine, 9