If we model a robot as multiple links connected all together through rotating joints we get something like the following figure. For simplicity the robot is represented with only two links but the following equations could be generalized to robot with more links. The robot is connected to the ground in A which we will define as $(0, 0)$. The length of each link will be represented by $l_1$, $l_2$ and so on.

In the previous post I describe how I made a collision checking function between multiple line segments and circles. Here is the code: Dot ← +´∘× # 𝕩≢n‿2‿2 𝕨≢⟨k‿2, k⟩ ColSegmentsCircles ← { [a,b] ← 1⍉𝕩 ⋄ c‿r ← 𝕨 ⋄ r_squared ← r⋆2 ca ← {𝕩 -⎉1 c}⎉1 a ⋄ cb ← {𝕩 -⎉1 c}⎉1 b col ← {∨´⎉1}⍟2 r_squared≥⎉1 Dot˜⎉1 {𝕩 -⎉1 c}⎉1 𝕩 n ← (-⌾⊑)∘⌽˘ b-a d_squared ← (2⋆˜ n {𝕨 Dot⎉1 𝕩}˘ ca ) ÷ Dot˜˘ b-a col ↩ col ∨˘ ∨´˘ (0 ≥ ca Dot˘˘ cb) ∧ {r_squared ≥¨ 𝕩}˘ d_squared } This code will be used as the base for the path planners and will be the computation bottleneck.

I made an RRTConnect path planner but only used an obstacle in the configuration space. It was convenient for testing the planner but usually we do not know where the obstacles are located in the configuration space. Instead, we observe the obstacles in the robot workspace and they implictly define invalid states in the configuration space. In this post we will begin the work to implement a realistic collision checking function.