Using Motor Control Interfaces in Python

Goal

This tutorial shows how to use Python to interface with the iCub. In particular we focus on controlling the motors using the motor control interfaces. We assume that you have already compiled or installed the Python bingings. This tutorial assume basic knowledge of how YARP works in C++. We just show how the robot C++ interface can be used in Python.

Dislaimer: this code does not do anything fancy with the robot. We let you have fun with the robot and Python once you know how the interface works.

Joint level motor control

For this tutorial we only need the yarp python bindings.

First initialize yarp.

import yarp;
yarp.Network.init();

We know initialize a remote_controlboard object connected to the right_arm.

# prepare a property object
props = yarp.Property()
props.put("device","remote_controlboard")
props.put("local","/client/right_arm")
props.put("remote","/icubSim/right_arm")
 
# create remote driver
armDriver = yarp.PolyDriver(props)

Now we can query the motor interfaces.

#query motor control interfaces
iPos = armDriver.viewIPositionControl()
iEnc = armDriver.viewIEncoders()

We are now ready to retrive the number of joints, create vectors to store the position commands and move the arm.

#retrieve number of joints
jnts=iPos.getAxes()
 
print 'Controlling', jnts, 'joints'
 
# read encoders
encs=yarp.Vector(jnts)
iEnc.getEncoders(encs.data())
 
# store as home position
home=yarp.Vector(jnts, encs.data())
 
#initialize a new tmp vector identical to encs
tmp=yarp.Vector(jnts)
 
# add 10 degrees to the first four joints
tmp.set(0, tmp.get(0)+10)
tmp.set(1, tmp.get(1)+10)
tmp.set(2, tmp.get(2)+10)
tmp.set(3, tmp.get(3)+10)
 
# move to new position
iPos.positionMove(tmp.data())

Now wait some time and go back to home position:

iPos.positionMove(home.data())

Code can be found at python/python-motor-control.py

This file can be edited at doc/python-motor-control.dox