US Patent 8396595 Real-time self collision and obstacle avoidance using weighting matrix

A system, method, and computer program product for avoiding collision of a body segment with other structures in an articulated system are described. According to one aspect, a collision function is determined for avoiding such collision. A distance between the body segment and one such structure is measured. A weighting matrix is generated based on the collision function and the distance, and used to determine a redirected motion for the body segment. The body segment is redirected based on the redirected motion to avoid colliding with the structure.