Constrained Closed Loop Inverse Kinematics Behzad Dariush Youding Zhu Arjun Arumbakkam Kikuo Fujimura Abstract— This paper introduces a kinematically constrained closed loop inverse kinematics algorithm for motion control of robots or other articulated rigid body systems. The proposed strategy utilizes gradients of collision and joint limit potential functions to arrive at an appropriate weighting matrix to penalize and dampen motion approaching constraint surfaces. The method is particularly suitable for self collision avoidance of highly articulated systems which may have multiple collision points among several segment pairs. In that respect, the proposed method has a distinct advantage over existing gradient projection based methods which rely on numerically unstable null-space projections when there are multiple intermittent constraints. We also show how this approach can be augmented with a previously reported method based on redirection of constraints along virtual surface manifolds. The hybrid strategy is effective, robust, and does not require parameter tuning. The efficacy of the proposed algorithm is demonstrated for a self collision avoidance problem where the reference motion is obtained from human observations. We show simulation and experimental results on the humanoid robot ASIMO. I. I NTRODUCTION Motion planning with kinematic constraints has been an important and widely studied problem since the inception of robotics technology. The majority of early research in this area was focused on obstacle avoidance, typically for applications involving mobile robot navigation and industrial manipulation [1], [2]. In these applications, the workspace was often predefined, static, or slowly varying. Moreover, application developers typically adopted the philosophy of segregating the workspace of robots and humans as a safety countermeasure to avoid collisions with humans. Today, the field of robotics is moving towards development of high degree of freedom, human-like, and personal robots, which are often designed to share a common workspace and physically interact with humans. Such robots are often highly redundant which fundamentally adds new capabilities (self-motion and subtask performance capability). However, increased redundancy has also added new challenges for constraining the internal motion to avoid joint limits and self collisions. With these challenges, researchers have become increasingly aware of the need for robust joint limit and colli- sion avoidance strategies to accommodate such applications. In particular, self collision avoidance, which was largely overlooked or not required when obstacle avoidance strate- gies were first developed, has recently become an impor- tant topic of research [3], [4], [5]. Enforcing self collision B.Dariush, A. Arumbakkam, and K. Fujimura are at the Honda Research Institute, USA, 800 California St. Suite 300, Mountain View CA 94041 dariush(aarumbakkam)(kfujimura)@honda-ri.com Y. Zhu is at Department of Computer Science, The Ohio State University, zhu.81@osu.edu constraints is challenging for humanoid robots performing human-like tasks, especially in a real-time or online setting. The strategy must not only accommodate multiple colliding segments simultaneously, but also tolerate smaller collision distance thresholds than those established for early obstacle avoidance algorithms. In addition, such constraints should not significantly alter the reference or originally planned motion. This is particulary important in applications in- volving reproduction of robot motion from observed human motion [5], [6], [7], [8]. This paper introduces an online, kinematically constrained motion generation algorithm for motion control of robots or other articulated rigid body systems in task space. The strategy is formulated in the framework of the closed loop inverse kinematics (CLIK) algorithm [9]. The presented CLIK formulation is based on a weighted and regularized pseudo-inverse solution which computes joint variables given a set of motion descriptors specified in Cartesian space. The first contribution of this paper is the construction of an appropriate weighting matrix which results in collision free motion. The weighting matrix is based on the gradient of a potential function which penalizes and dampens joints whose motion directs the segments toward joint limit and collision constraints. The proposed method is particularly suitable for self collision avoidance of highly articulated systems which may have multiple and changing collision points among several segments. In this respect, the proposed method has a distinct advantage over existing gradient projection based methods which are susceptible to numerical instability when dealing with multiple and intermittently colliding segment pairs [1], [10]. The second contribution of this paper is to show that the proposed method may be augmented with our previously re- ported collision avoidance strategy to improve robustness [5]. In particular, the hybrid strategy is effective in guaranteeing collision free motion without the need to tune parameters for the construction of collision potential functions. To demonstrate the effectiveness of the proposed algo- rithm, we illustrate simulated and experimental results on the Honda humanoid robot ASIMO, where the reference motion is obtained from captured human motion. The reference motions are complex, fast, and exhibit frequent self collisions under the traditional CLIK motion generation. With the proposed algorithm, the motions are shaped in real time to produce kinematically constrained motions. The algorithm is also implemented in our online motion retargeting framework and demonstrated on the ASIMO platform. 2010 IEEE International Conference on Robotics and Automation Anchorage Convention District May 3-8, 2010, Anchorage, Alaska, USA 978-1-4244-5040-4/10/$26.00 ©2010 IEEE 2499