Patent ID: 8396595

Claim:
A method for avoiding collision of a body segment in an articulated system, the method comprising: determining a collision function for avoiding collision between the body segment and other structures in the articulated system; measuring a distance between the body segment and a first structure, the first structure being one of the other structures; determining a joint limit function for avoiding self collision between the body segment and the other structures in the articulated system; measuring a joint vector between the body segment and a second structure, the second structure being one of the other structures; generating, using a processor, a collision gradient weighting matrix based on the collision function and the measured distance; generating, using a processor, a joint limit gradient weighting matrix based on the joint limit function and the measured joint vector; generating a combined weighting matrix based on the collision gradient weighting matrix and the joint limit gradient weighting matrix; determining a redirected motion that prevents the body segment from colliding with the first structure based on the combined weighting matrix; and redirecting the body segment based on the redirected motion to avoid colliding with the first structure.