Controlling Joints

Brains evolved to control bodies. Some of that control (homeostasis) is about keeping physiological parameters in balance, controlling metabolism to regulate temperature and calorie levels. The rest is about planning movements to obtain the things we want, such as food and water, and to avoid the things we don’t, such as becoming the food of something else.

We plan these kinds of movements effortlessly, with reference to several different neural representations of the body, known by psychologists as the ‘body schema’. At the highest level, we maintain a body image, which concerns the emotions we have towards our bodies. At the lower levels, we maintain representations of the body topology (which parts are connected to which), its surface structure (the distance between points on the skin), and its three-dimensional posture (incorporating joint angles and limb lengths). Evidence that these neural representations are distinct, and about how they interact, comes from a wealth of fascinating experiments. If you are interested, search for “Phantom limb”, “Rubber hand illusion”, or “Pinnochio illusion” to explore further.

In robotics, the lowest level body representation – the postural schema – is maintained explicitly, in the form of the robot’s kinematic tree. The kinematic tree contains a list of the names of each link and joint, as well as the name of the joint to which each link is connected, the points at which those connections are made, the axis of movement enabled by each joint, and whether the joint allows movement along that axis (for ‘linear’ joints) or about that axis (for ‘rotational’ joints). This structure contains all the information required to calculate the positions and orientations of its links, when we supply a list of the current joint settings.

The calculation is known as forward kinematics, and it involves two steps. First, a transform matrix (a four-by-four table of numbers) is constructed for each joint, which contains its location on the link (3 numbers) and the movement axis (3 numbers), and trigonometry is used to offset those numbers by the current joint settings. Then the values in those matrices are multiplied in a sequence that reflects the relative position of the corresponding joints in the tree (matrices of adjacent joints are multiplied consecutively).

Learning how to calculate forward kinematics is a foundational part of any degree course in robotics, and certainly a worthwhile exercise, but the ability to program these calculations from scratch should no longer be a barrier to getting started in robotics.

The BOW SDK performs these forward kinematics calculations behind the scenes, maintaining a simple model of how the robot’s links are arranged in space, using a common set of mathematical operations that work in exactly the same way, irrespective of the robot’s form factor. In this tutorial we will start to explore the kinematic trees of different robots, and to position them by sending motor messages to set joint angles directly.

Take a look at the following to see what we're aiming for:

About the Application

The purpose of the application you will develop here is to enable users to explore the basic movement capabilities of their robots. The structure of the application is as follows:

Before we get stuck in:

If you are just browsing to get a sense of what's possible, take a look at the code online.

Running the Application

cd SDK-Tutorials/Control/ControllingJoints/Python

python main.py

Investigation

Once the tutorial is running, a simple menu interface will launch in the terminal. Each of the robot's joints will be listed, and you can use the up and down arrows on your keyboard to scroll between them. Pressing the left or right arrow key will increment the joint angle for the selected joint. The example code automatically creates a slider display for each of your robot's joints. It obtains the names of each joint, and the values that correspond to their maximum and minimum positions, from an initial call to myrobot.proprioception.get. When you press the left or right key, a motor message is constructed and sent to the robot by calling myrobot.motor.set.

Let's take a closer look at the key parts of the code:

jointList = []
while True:
    prop_msg, err = robot.proprioception.get(True)
    if err.Success and len(prop_msg.RawJoints) > 0:
        for joint in prop_msg.RawJoints:
            if joint.Type == bow_data.Joint.FIXED:
                continue
            jointList.append(display.DisplayInfo(joint.Name,joint.Min,joint.Max,joint.Position))
        break
    time.sleep(0.1)

    class myDisplay(display.Display):
	    def SetCurrentJoint(self):
            joint = self.list[self.listIndex]
            motorSample = bow_data.MotorSample()
            motorSample.ControlMode = bow_data.MotorSample.USE_DIRECT_JOINTS
            motorSample.RawJoints.append(bow_data.Joint(Name=joint.Name, Position=joint.Value))
            robot.motor.set(motorSample)
            time.sleep(0.03)

Try appending a new instruction for one of the joints to the raw joints structure that is sent as part of the motor message. The example code sets the Position of the joint using the current slider value, but you could experiment with other control types, for example Velocity, Acceleration, Torque, or a combination.