jpowcode

In this post I will describe how I calculate the servo postions to place the tip of the arm at some specified coordinates. For example, if the arm needs to pick something up off the ground in a specific place there will be several combinations of joint positions that will get the arm to that place. I created a simple app using Geogebra to demonstrate and test different joint positions. Moving the sliders changes the length of the joint position and gabbing and moving a point will change the joint angles. This will be useful for testing the code that controls the arm. This app can also be viewed on Geogebratube

To calculate the joint positions I have adapted a programme created by studywolf to work for a 4 degree of freedom arm. The programme takes as input the current position of the arm and the desired position of the arm and uses an optimisation algorithm from the scipy library to find joint positions that require the smallest change possible to reach the desired position. I've decommented the code, broken it up and described it below. The original code is on studywolfs github page.

We need to import math, numpy and scipy.optimise libraries. The init method of the class checks whether any parameters have been passed and creates any that are missing. The inputs are

• q - an array holding the initial joint positions.
• q0 - an array holding the resiting joint positions
• L - an array holding the lengths of the joints.

Two arrays max_angles and min_angles are also created to provide limits of movement on the arm's joint positions to prevent it from over extending or moving to positions that are restricted by brackets.

The get_xy method takes as input an array of joint positions and calculates the (x,y) coordinates of the the end of the arm using some trigonometry. Details on studywolf's blog.

The inv_kin method is the one that does all the grunt work using the scipy.optimise function.

distance_to_default is the function that will be minimised. It is a measure of how far all the joints will have to move. The weight array puts heavier penalties on some joints moving that others.

x_constraint calculates the distance in the x-direction from the current position to a new position with given joint angles. y_constraint does the same in the y direction.

To get all of this to work we create a function findJointPos that creates an instance of the class and calls the inv_kin method. It returns an array cotaining the optimal joint positions to place the arm in this positon. For example if we call findJoinPos(2, 2) and we get the angles [-0.33, 0.84, 0.56, 0.80] which are in radians. Converting these to degrees gives approximately [-19, 48, 32, 46]. Using the Geogebra demo above to set the angles we can see that the arm does indeed get to the point (2,2).