This paper proposed a motion planning and control approach for real-time collision avoidance of high degree-of-freedom (DoF) robots. It constructs a 3D visibility graph by representing external obstacles with discrete polyhedrons and searches for the shortest path on it. To consider the complex internal structure of high DoF
robots and take full advantage of redundancy, inverse kinematics is solved as an optimization problem which allows for flexible cost functions and constraints. The distance between the robot and external obstacles is treated as an extra constraint in the optimization which works together with visibility graph for collision avoidance. At the end, a pick-place task was successfully performed among obstacles by a simulated dual-arm quadrupedal robot considering the whole-body kinematics and stability, which demonstrated the feasibility of the proposed method.