Creation date: 2021-09-08

Time for a new post in this Javis Examples Series. Last time we had a look at a problem that we wanted to visualize. This time we just want to create an interesting animation while we are learning about inverse kinematics. It's a topic tackled by a bunch of other bloggers and YouTubers out there already but I hope you'll still enjoy my animated visualizations and the end result in Javis . Maybe you can create your own animations with it 😉

We have a robot arm which consists of several segments. Now we want to grab an object from which we obtained the position for example by computer vision. The question is now how to arrange the segments/which angle to choose at the joints such that the last segment in the arrangement reaches the object.

Fortunately the algorithm for this is rather simple and as you have now seen also very practical.

Let me introduce you to the algorithm with a small description and an animation. After that I'll also share the code for the animation with you. As you all know: This is a Javis series 😄

We'll do this with 3 segments for the arm later but it can be however many you want. Let's first assume that we only have one segment though. That might point up at the beginning but the object is to the right. Maybe we make the problem even simpler shall we? Let say we can move the arm around freely so it is not attached to the robot.

Now the solution seems quite simple and obviously there are infinitely many solutions. The head of the segment just needs to be at object and the length of the segment is defined but the start or the angle of the segment can be arbitrarily. However it seems rather helpful to not completely change the angle of the at the moment still upwards pointing segment. Instead we want to change the angle in the direction of the object.

Here you can see one segment of the arm rotating around until it is pointing directly towards the point. Then we just do a simple translation to the point itself. The recursive idea is now to do this for the next segment as well but it doesn't want to reach the object but instead the start of the last segment.

The angle here can be evaluated by using the
` atan `

function, which gives us an angle between \(-\pi\) and \(\pi\) when called with two arguments. In other languages this is sometimes called `atan2`

.

We need to call it with: `atan(p.y-seg.start.y, p.x-seg.start.x)`

where \(p\) is the
` Point `

we want to reach and `seg`

is our segment with start as its tail.

I'll go into how we make sure that our segment which is attached to the robot doesn't move after the next section. Let's get into the coding part.

In this section I want to start introducing the basics of the code we need to create a nice little animation for inverse kinematics. Then I'll explain locking the first segment to a point for a stationary robot and also add that part to the implementation.

We start as always with the basics of `using Javis`

and the `ground`

function. Also add a `Segment`

struct.

```
using Javis
mutable struct Segment
start :: Point
angle :: Float64
length :: Float64
head :: Point
end
function ground(args...)
background("black")
sethue("white")
end
function main()
vid = Video(512, 512)
nframes = 200
Background(1:nframes, ground)
render(vid; pathname="arm.gif")
end
```

Each segment just has a start point, an angle, a length and a head point. You only really need two of them or three depending on which one you choose but I found it easier to work with those four 😉

Let's create a single segment first and draw it.

In `main`

:

```
seg = Segment(O, 0, 100, Point(100, 0))
seg_obj = Object(1:nframes, draw_segment(seg))
```

and the function:

```
function draw_segment(seg::Segment)
@JShape begin
line(seg.start, seg.head, :stroke)
end
end
```

Now we don't want to compute the `head`

all the time if we have the other info so let's create a constructor for `Segment`

and a `sethead!`

function to calculate the missing info.

```
function Segment(start, angle, length)
s = Segment(start, angle, length, O)
sethead!(s)
return s
end
function sethead!(seg::Segment)
dx = cos(seg.angle)*seg.length
dy = sin(seg.angle)*seg.length
seg.head = seg.start + Point(dx, dy)
end
function setstart!(seg::Segment)
dx = cos(seg.angle)*seg.length
dy = sin(seg.angle)*seg.length
seg.start = seg.head - Point(dx, dy)
end
```

then we can use

`seg = Segment(O, 0, 100)`

I've also added `setstart!`

just that we have a function for the other way around.

Now let's start with the actual code. We want to have a `reach!`

function which does take a segment and a point and rotates the segment towards the point and translate to reach it in the end.

```
function reach!(seg::Segment, p::Point)
sethead!(seg)
seg.angle = atan(p.y-seg.start.y, p.x-seg.start.x)
seg.head = p
setstart!(seg)
end
```

Here we make sure to compute the head from the given info. Then we compute the angle using the
` atan `

function as discussed before. The translation is done by simply fixing the `head`

to `p`

and then call the `setstart!`

function.

Now if we call `reach!(seg, Point(100, 100))`

before calling the
` Object `

we would get this:

I've also added this to show the object itself:

`Object(1:nframes, JCircle(Point(100, 100), 5; color="blue", action=:fill))`

You might want to check out the
` JCircle `

function which is relatively new to Javis. You can specify the position, radius the color and whether it should be filled or just outlined (`:stroke`

)

Before we add more segments let's do a little bit of animation with what we have by moving the point.

We need to call the `reach!`

function for this inside the `draw_segment`

function and pass a point to it and be able to change it with
` change `

.

```
function draw_segment(seg::Segment, p::Point)
@JShape begin
reach!(seg, p)
line(seg.start, seg.head, :stroke)
end p=p
end
```

The `p=p`

at the end of
` @JShape `

makes it possible to change the `p`

later using
` change `

.

Now let's change the `p`

in a continuous fashion with:

```
p = Point(100, 100)
seg_obj = Object(1:nframes, draw_segment(seg, p))
obj = Object(1:nframes, JCircle(Point(100, 100), 5; color="blue", action=:fill))
act!(seg_obj, Action(1:nframes, change(:p, p => Point(190, 20))))
act!(obj, Action(1:nframes, change(:center, p => Point(190, 20))))
```

Okay let's do this! One segment is boring, isn't it? Let's add some more to see it in action. First we change `seg = Segment(O, 0, 100)`

to `segs = [Segment(O, 0, 100), Segment(Point(100,0), 0, 75), Segment(Point(175,0), 0, 50)]`

Then we also pass `segs`

to the `draw_segment`

function and while we are at it: Maybe rename it to `draw_arm`

. Don't worry I'll share the whole code at the end of the post 😉

The main change for this besides also drawing the segments in the for loop is the creation of this function:

```
function reach!(arm::Vector{Segment}, p::Point)
for i in length(arm):-1:1
reach!(arm[i], p)
p = arm[i].start
end
end
```

We start with the last segment and call the old `reach!`

function on the initial point. Then we change the point to the start of the newly placed segment and go down in the list of segments.

You can see in the animation above though that the base of the arm isn't stationary though. If we can move the robot itself in the two dimensions we have, why do we even need an arm then? 😄

Let's fix that in the next section. It's simpler than you might think.

I said it's simpler than you think but let me start with something that is even simpler than it actually is 😄

We simply do all the stuff we did before and then move the base of the arm to where it should be.

Yeah that is mostly it but as you can imagine it can't really work because if we move it to that place and all segments attached to it, the hand (last segment which grabs the object... am I really explaining what a hand is? 😄 ) will actually not grab the object anymore but it turns out that once the base is actually at the locked position we are always quite close to the target. (Given that the length of the arm is long enough).

What we can do to actually grab the object is the following: Iteration! Basically just doing the same thing over and over again until we are in a given error margin.

There is probably a better way to write this but I've changed the `reach!`

function to this:

```
function reach!(arm::Vector{Segment}, p::Point)
end_point = p
locked_point = arm[1].start
loop = false
if distance(locked_point, end_point) <= sum(s->s.length, arm)
loop = true
end
loop_counter = 0
while loop_counter == 0 || loop
p = end_point
for i in length(arm):-1:1
reach!(arm[i], p)
p = arm[i].start
end
# move the arm back
arm[1].start = locked_point
sethead!(arm[1])
for i in 2:length(arm)
arm[i].start = arm[i-1].head
sethead!(arm[i])
end
# check such that we don't get into an infinite loop or are already close enough
loop_counter += 1
if loop_counter == 10 || distance(end_point, arm[end].head) <= 0.1
break
end
end
end
```

First we save the point we want to reach and the locked point of the base. Then we check whether we can reach the point by just computing whether the arm is long enough. If that isn't the case we only want to run the inner call ones, as we still want point into the right direction.

Then we do our normal reaching stuff and then in `# move the arm back`

we set the start of our first segment and the head and do that for the remaining segments such that they are all connected.

At the end we check whether we reached our goal or whether we have to go into a second/third/... loop. We don't want to get stuck in an infinite loop so I've set the break point after 10 calculations.

Now we know all we need to know to create stunning animations using inverse kinematics.

My idea was the following: I want to somehow see the different angles of the three segments as a color and let the robot more or less paint something. I think moving line by line or column by column isn't very interesting therefore I decided to use space filling curves like the Hilbert Curve.

Let's maybe start with not moving the object but instead create a circle at each frame which stays there.

For this I simply added the following to the `draw_arm`

function and also created an empty vector `points`

before creating the
` Object `

. You can read more about this in our first Javis tutorial.

```
push!(points, segs[end].head)
sethue("blue")
for point in points
circle(point, 5, :fill)
end
```

Making them all blue is probably a bit lame so let's create a struct `ColoredPoint`

.

```
struct ColoredPoint
p :: Point
c :: HSL{Float64}
end
```

for the colorspace `HSL`

we need `using Colors`

.

Then we get the color with something like this:

```
function get_color(arm::Vector{Segment})
an = [(seg.angle+π)/2π for seg in arm]
return HSL(an[1]*360, an[2], an[3]*0.8+0.2)
end
```

Feel free to create your own version with this. The first line here is to have a normalized vector from `0`

to `1`

of the angles.

The idea is to compute the color based on the angles and instead of storing the `Point`

vector as above we will have a `ColoredPoint`

vector and draw by calling
` sethue `

with the specified color.

Now the last step is to create the Hilbert curve. For this I used the package: HilbertSpaceFillingCurve which is rather old but still functional.

It has the function `hilbert(d::T, ndims, nbits = 32)`

where `d`

is basically the index ranging from `0`

to `(2^nbits)^ndims-1`

. It will give you back a vector of `ndims`

dimensions and a part of a coordinate is in the range of `0`

to `2^nbits-1`

. Maybe it will be clearer in our specific example. I choose the dimensions of the image as 512x512 which means each dimension is `2^9`

such that we can set `nbits = 9`

. Now we want to fill the whole space which means we have `512^2`

values for the index `d`

which is over 250,000. I don't want to have that many frames such that I decided to have 2000 frames which is a bit over one minute for 30fps. And just step over a couple of indices to not actually fill the complete space 😄

Well that was kinda unexpected 😂 Well it seems like the arm isn't long enough but as a matter of fact: I like this one even more than the square one I created before so I leave it like that.

I don't want to bother you with the last implementation details as you can find the whole code here but I think the last thing I wanna mention is that you can use Animations to define the transitions of one point to another and pass it over to the
` change `

function like this:

```
anim_p = Animation(
[i/(length(ps)-1) for i in 0:length(ps)-1], # must go from 0 to 1
ps,
[linear() for _ in 1:length(ps)-1],
)
act!(arm_obj, Action(1:nframes, anim_p, change(:p)))
```

In this case the `ps`

are the points created by `hilbert`

and we don't really need the transitions as we jump from one to another in one frame but it's still easier to pass it to the
` change `

function this way.

Alrighty! Thanks everyone for checking out my blog again and see you soon!

And don't forget to subscribe to my free email newsletter to not miss a post.

Or well you can go one step further if you have some money to spare 😉

**Thanks to my 12 patrons!**

Special special thanks to my >4$ patrons. The ones I thought couldn't be found 😄

Anonymous

Kangpyo

Gurvesh Sanghera

Szymon Bęczkowski

Colin Phillips

Jérémie Knuesel

For a donation of a **single** dollar per month you get early access to these posts. Your support will increase the time I can spend on working on this blog.

There is also a special tier if you want to get some help for your own project. You can checkout my mentoring post if you're interested in that and feel free to write me an E-mail if you have questions: o.kroeger <at> opensourc.es

I'll keep you updated on Twitter OpenSourcES.

Want to be updated? Consider subscribing and receiving a mail whenever a new post comes out.

© Ole Kröger. Last modified: September 18, 2021. Website built with Franklin.jl.