The main idea is that adding external forces to the joint points of the robotic arm. If any obstacle (object) is close enough to the robot, the algorithm adds an external linear force and this force causes an acceleration in the same direction. The algorithm generates and converts this directional external force into joint torques. I originated from the “Featherstone Algorithm” which is also called as “Articulated Body Algorithm” because of this purpose. And also to decide how to generate these external forces around joint points, I originated from Oussama Khatib’s “Potential Field Algorithm", but I did not use the same algorithm and formulas, usually I created my approach to decide forces and to convert into the joint torques.
There is a "V-Rep" Scene, which includes one "Kuka Iiwa" robotic arm and two obstacles (Red and Yellow ones). C++ code is running in the background. This program sends joint angles of the robotic arm to the "V-Rep" and drive the robot and gets the obstacle position information from V-Rep only. All other kinds of stuff are done by the algorithm works on the C++ code. If the obstacle is close enough to the robot, the robot performs an avoidance behavior. The other one causes the same result too. The robot is trying to avoid obstacles as far as it can, as you can see. I applied an interpolation process to the obstacles. As a result of this, the current position of the obstacle applies a force to the robot but at the same time, to get a better result for moving objects the algorithm estimates the future position of the obstacle and applies forces with respect to the estimated future positions too. This provides more determined movements. Before the obstacle enters the avoidance area, the robot reacts.
I have coded some safety measures for robots. If any joint of the robot takes a position close less than 15 cm to the floor, Program shut itself down and in the area of more than 15 cm but less than 35 cm close to the floor, the floor applies force upper direction to the robot, like an obstacle. Any joint can not pass the area because of this force. If it passed anyway, the program shut itself down. As a result of this emergency measure, the probability of crush to the floor has been eliminated.
While the algorithm calculating the angles, torque values cause a rotational acceleration, and the algorithm calculates acceleration then velocity then the angle. In the light of the "Kuka Iiwa Robotic Arm" documentation, I have saturated angles and velocity values to the max capability of the robot. So you can not see any unrealistic movement in this simulation, I don't send this command.