18 Motor, 6 Legged robot walking using Python / ROS

Question

I have been using ROS alongside Python to enable my built robot to walk. I am aware that my code needs some improvement such as classes etc. This is my first Python / ROS program so any help and pointers would be hugely appreciated.

#!/usr/bin/env python
#
## launch start_robot_controller.launch before running this.
import roslib; roslib.load_manifest('my_dynamixel_tutorial')
import rospy
import math
import operator
import sys
import select
#import msvcrt
#import pygame
import termios, sys, os
TERMIOS = termios
from std_msgs.msg import Float64
from dynamixel_msgs.msg import MotorStateList
from dynamixel_msgs.msg import JointState
from geometry_msgs.msg import Twist
## define the command publishers for joints, Legs are named as: L - left; R - Right; M - middle; R - rear. each leg has three joints, 1,2,3
pub_LF1 = rospy.Publisher("/LF1/command", Float64, latch=True)
pub_LF2 = rospy.Publisher("/LF2/command", Float64, latch=True)
pub_LF3 = rospy.Publisher("/LF3/command", Float64, latch=True)
pub_LM1 = rospy.Publisher("/LM1/command", Float64, latch=True)
pub_LM2 = rospy.Publisher("/LM2/command", Float64, latch=True)
pub_LM3 = rospy.Publisher("/LM3/command", Float64, latch=True)
pub_LR1 = rospy.Publisher("/LR1/command", Float64, latch=True)
pub_LR2 = rospy.Publisher("/LR2/command", Float64, latch=True)
pub_LR3 = rospy.Publisher("/LR3/command", Float64, latch=True)
pub_RF1 = rospy.Publisher("/RF1/command", Float64, latch=True)
pub_RF2 = rospy.Publisher("/RF2/command", Float64, latch=True)
pub_RF3 = rospy.Publisher("/RF3/command", Float64, latch=True)
pub_RM1 = rospy.Publisher("/RM1/command", Float64, latch=True)
pub_RM2 = rospy.Publisher("/RM2/command", Float64, latch=True)
pub_RM3 = rospy.Publisher("/RM3/command", Float64, latch=True)
pub_RR1 = rospy.Publisher("/RR1/command", Float64, latch=True)
pub_RR2 = rospy.Publisher("/RR2/command", Float64, latch=True)
pub_RR3 = rospy.Publisher("/RR3/command", Float64, latch=True)
## leg states. each leg has three states: 1 - move ofrward in the air; 2 - push down to touch ground; 3 - move backward on the ground. 
## use only middle legs, LM and RM, to monitor the state. LM and RF and RR are in one tripod, thus the same state. 
global LM_state
global RM_state
rad = 180.000001/3.1415926
## define the desired positions of each state for each joint, below are only for left legs. The values for right leges should be multiplied by -1, because the motors at two legs are mirrored to each other.
AEP = -20 # anterior extreme position of joint 1, in degree
PEP = 20 # posterior extreme position of joint 1, in degree
# walk with higher body altitude ## with this, robot sometimes stop walking due to small delta value
#P_j1_s1 = AEP/rad ## state 1
#P_j2_s1 = -45/rad
#P_j3_s1 = -30/rad
#P_j1_s2 = AEP/rad ## state 2
#P_j2_s2 = 0/rad
#P_j3_s2 = 0/rad
#P_j1_s3 = PEP/rad ## state 3
#P_j2_s3 = 0/rad
#P_j3_s3 = 0/rad
## OR: walk with lower body altitude
P_j1_s1 = AEP/rad ## state 1
P_j2_s1 = -65/rad
P_j3_s1 = -30/rad
P_j1_s2 = AEP/rad ## state 2
P_j2_s2 = -45/rad
P_j3_s2 = -20/rad
P_j1_s3 = PEP/rad ## state 3
P_j2_s3 = -45/rad
P_j3_s3 = -20/rad
## define the desired positions of all three joints in each leg
# walk with medium body altitude,, With this, robot walking looks better
#P_j1_s1 = AEP/rad ## state 1
#P_j2_s1 = -60/rad
#P_j3_s1 = -45/rad
#P_j1_s2 = AEP/rad ## state 2
#P_j2_s2 = -30/rad
#P_j3_s2 = -20/rad
#P_j1_s3 = PEP/rad ## state 3
#P_j2_s3 = -30/rad
#P_j3_s3 = -20/rad
## indicate whether the joints have reached the desired positions in middle legs
global flag_LM1
global flag_LM2
global flag_LM3
global flag_RM1
global flag_RM2
global flag_RM3
## desired position of each joint
global p_LM1 
global p_LM2 
global p_LM3 
global p_LF1 
global p_LF2 
global p_LF3 
global p_LR1 
global p_LR2 
global p_LR3 
global p_RM1 
global p_RM2 
global p_RM3 
global p_RF1 
global p_RF2 
global p_RF3 
global p_RR1 
global p_RR2 
global p_RR3 
global robot_moving
global Ra_L ## turning ratio for left leg
global Ra_R
## turning rate for left and right turning
Ra_L = 1 ## if being 1, means no turning to left
Ra_R = 1
robot_moving = 1 # move forward
LM_state = 1
RM_state = 3 ## at the beginning, initiate the gait with right middle leg and its tripod.
## set the initial desired pos for each joint according to the above LM_state and RM_state values
# because LM_state = 1, so:
p_LM1 = P_j1_s1
p_LM2 = P_j2_s1
p_LM3 = P_j3_s1
p_RR1 = (-1)*P_j1_s1
p_RR2 = (-1)*P_j2_s1
p_RR3 = (-1)*P_j3_s1
p_RF1 = (-1)*P_j1_s1
p_RF2 = (-1)*P_j2_s1
p_RF3 = (-1)*P_j3_s1 
# because RM_state = 3, so:
p_RM1 = (-1)*P_j1_s3
p_RM2 = (-1)*P_j2_s3
p_RM3 = (-1)*P_j3_s3
p_LR1 = P_j1_s3
p_LR2 = P_j2_s3
p_LR3 = P_j3_s3
p_LF1 = P_j1_s3
p_LF2 = P_j2_s3
p_LF3 = P_j3_s3 
global cmd
cmd = 2.0
global count 
count = 0
count2 = 0
## the functions below monitor whether the middle leg joints have reached the desired pos
delta = 2.00/rad ## minimum error allowed, in radius. when below this error, we assume the desired position is reached
def LM1_callback(msg):
 global flag_LM1
 global p_LM1
 flag_LM1 = 0 ## means not reaching the desired pos
 if abs(msg.current_pos-p_LM1)<delta:
 #if msg.is_moving == False:
 #print 99
 flag_LM1 = 1
 #print flag_LM1
def LM2_callback(msg):
 global flag_LM2
 global p_LM2
 flag_LM2 = 0 ## means not reaching the desired pos
 if abs(msg.current_pos-p_LM2)<delta:
 #if msg.is_moving == False:
 flag_LM2 = 1
def LM3_callback(msg):
 global flag_LM3
 global p_LM3
 flag_LM3 = 0 ## means not reaching the desired pos
 if abs(msg.current_pos-p_LM3)<delta:
 #if msg.is_moving == False:
 flag_LM3 = 1
def RM1_callback(msg):
 global flag_RM1
 global RM_1
 flag_RM1 = 0 ## means not reaching the desired pos
 if abs(msg.current_pos-p_RM1)<delta:
 #if msg.is_moving == False:
 flag_RM1 = 1
def RM2_callback(msg):
 global flag_RM2
 global p_RM2
 flag_RM2 = 0 ## means not reaching the desired pos
 if abs(msg.current_pos-p_RM2)<delta:
 #if msg.is_moving == False:
 flag_RM2 = 1
def RM3_callback(msg):
 global flag_RM3
 global p_RM3
 flag_RM3 = 0 ## means not reaching the desired pos
 if abs(msg.current_pos-p_RM3)<delta:
 #if msg.is_moving == False:
 flag_RM3 = 1
 global flag_LM1
 global flag_LM2
 global flag_LM3
 global flag_RM1
 global flag_RM2
 global LM_state
 global RM_state
 ## desired position of each joint
 global p_LM1 
 global p_LM2 
 global p_LM3 
 global p_LF1 
 global p_LF2 
 global p_LF3 
 global p_LR1 
 global p_LR2 
 global p_LR3 
 global p_RM1 
 global p_RM2 
 global p_RM3 
 global p_RF1 
 global p_RF2 
 global p_RF3 
 global p_RR1 
 global p_RR2 
 global p_RR3
 global robot_moving
 global Ra_L
 global Ra_R 
 global count 
 count = count + 1
 print 
 print
 print count 
 print flag_LM1, flag_LM2, flag_LM3, flag_RM1, flag_RM2, flag_RM3
 # indicating whenther the middle legs have reached the end of its current state
 flag_LM = flag_LM1*flag_LM2*flag_LM3 # left middle
 flag_RM = flag_RM1*flag_RM2*flag_RM3 # right middle
 ## here are the controls for the states of middle legs
 if LM_state==1 and flag_LM==1:
 LM_state = 2
 flag_LM = 0
 if LM_state==2 and flag_LM==1 and RM_state==3 and flag_RM==1:
 LM_state = 3
 RM_state = 1
 flag_LM = 0
 flag_RM = 0
 if LM_state==3 and flag_LM==1 and RM_state==2 and flag_RM==1:
 LM_state = 1
 RM_state = 3
 flag_LM = 0
 flag_RM = 0
 if RM_state==1 and flag_RM==1:
 RM_state = 2
 flag_RM = 0
 print LM_state, RM_state 
 ## calculate the desired position of each joint according to the state of the leg
 if LM_state == 1:
 p_LM1 = Ra_L*P_j1_s1
 p_LM2 = P_j2_s1
 p_LM3 = P_j3_s1
 p_RR1 = Ra_R*(-1)*P_j1_s1
 p_RR2 = (-1)*P_j2_s1
 p_RR3 = (-1)*P_j3_s1
 p_RF1 = Ra_R*(-1)*P_j1_s1
 p_RF2 = (-1)*P_j2_s1
 p_RF3 = (-1)*P_j3_s1 
 if LM_state == 2:
 p_LM1 = Ra_L*P_j1_s2
 p_LM2 = P_j2_s2
 p_LM3 = P_j3_s2
 p_RR1 = Ra_R*(-1)*P_j1_s2
 p_RR2 = (-1)*P_j2_s2
 p_RR3 = (-1)*P_j3_s2
 p_RF1 = Ra_R*(-1)*P_j1_s2
 p_RF2 = (-1)*P_j2_s2
 p_RF3 = (-1)*P_j3_s2 
 if LM_state == 3:
 p_LM1 = Ra_L*P_j1_s3
 p_LM2 = P_j2_s3
 p_LM3 = P_j3_s3
 p_RR1 = Ra_R*(-1)*P_j1_s3
 p_RR2 = (-1)*P_j2_s3
 p_RR3 = (-1)*P_j3_s3
 p_RF1 = Ra_R*(-1)*P_j1_s3
 p_RF2 = (-1)*P_j2_s3
 p_RF3 = (-1)*P_j3_s3 
 if RM_state == 1:
 p_RM1 = Ra_R*(-1)*P_j1_s1
 p_RM2 = (-1)*P_j2_s1
 p_RM3 = (-1)*P_j3_s1
 p_LR1 = Ra_L*P_j1_s1
 p_LR2 = P_j2_s1
 p_LR3 = P_j3_s1
 p_LF1 = Ra_L*P_j1_s1
 p_LF2 = P_j2_s1
 p_LF3 = P_j3_s1 
 if RM_state == 2:
 p_RM1 = Ra_R*(-1)*P_j1_s2
 p_RM2 = (-1)*P_j2_s2
 p_RM3 = (-1)*P_j3_s2
 p_LR1 = Ra_L*P_j1_s2
 p_LR2 = P_j2_s2
 p_LR3 = P_j3_s2
 p_LF1 = Ra_L*P_j1_s2
 p_LF2 = P_j2_s2
 p_LF3 = P_j3_s2 
 if RM_state == 3:
 p_RM1 = Ra_R*(-1)*P_j1_s3
 p_RM2 = (-1)*P_j2_s3
 p_RM3 = (-1)*P_j3_s3
 p_LR1 = Ra_L*P_j1_s3
 p_LR2 = P_j2_s3
 p_LR3 = P_j3_s3
 p_LF1 = Ra_L*P_j1_s3
 p_LF2 = P_j2_s3
 p_LF3 = P_j3_s3 
 ## send command to motors 
 if robot_moving==1: 
 pub_LF1.publish(p_LF1)
 pub_LF2.publish(p_LF2)
 pub_LF3.publish(p_LF3)
 pub_LM1.publish(p_LM1)
 pub_LM2.publish(p_LM2)
 pub_LM3.publish(p_LM3)
 pub_LR1.publish(p_LR1)
 pub_LR2.publish(p_LR2)
 pub_LR3.publish(p_LR3) 
 pub_RF1.publish(p_RF1)
 pub_RF2.publish(p_RF2)
 pub_RF3.publish(p_RF3)
 pub_RM1.publish(p_RM1)
 pub_RM2.publish(p_RM2)
 pub_RM3.publish(p_RM3)
 pub_RR1.publish(p_RR1)
 pub_RR2.publish(p_RR2)
 pub_RR3.publish(p_RR3) 
def led_control_demo():
 rospy.init_node('led_control_demo')
 #rospy.Subscriber("/motor_states/pan_tilt_port", MotorStateList, motor_callback)
 ### subscribe to the position feedbacks of middle leg joints.
 rospy.Subscriber("/LM1/state", JointState, LM1_callback)
 rospy.Subscriber("/LM2/state", JointState, LM2_callback)
 rospy.Subscriber("/LM3/state", JointState, LM3_callback)
 rospy.Subscriber("/RM1/state", JointState, RM1_callback)
 rospy.Subscriber("/RM2/state", JointState, RM2_callback)
 rospy.Subscriber("/RM3/state", JointState, RM3_callback) 
 ## run loops continuously
 rospy.spin()
if __name__ == '__main__':
 #rospy.init.node("led_control_demo")
 led_control_demo()

• SylvainD: Interesting question! I wish I could give it a try :-)
• 200_success: Follow-up question

Answer 1

So many globals

It doesn't seem like you quite realise what global does, so I'll explain how Python's scope works. When you call on a name, Python will search for it in the current scope it's in, but if it can't find it there it will search the next highest level of scope, to check there too. So, for example this code works fine:

foo = "I am global"
def bar():
 print foo
bar()
# I am global

foo is not declared as global, but Python will look for it in the global space after it realises that it's not declared inside bar.

Note, however, that this doesn't work if you try to set foo inside of bar but reference it before you set it. So this will work:

foo = "I am global"
def bar():
 foo = "I am local"
 print foo
bar()
# I am local

But this will not:

foo = "I am global"
def bar():
 print foo
 foo = "I am local"
bar()
# UnboundLocalError: local variable 'foo' referenced before assignment

So while you can read from any scope, it's only when you need to write to it that global is necessary. In the first case above, where foo is set to "I am local", the global foo is still it's original "I am global" value. If you want to change foo's value in the global scope that's where you need to call global foo.

foo = "I am global"
def bar():
 global foo
 foo = "I am redefined"
 print foo
bar()
# I am redefined
print foo
# I am redefined

Note that you don't need to call global foo outside of bar. In fact you never need to call global on anything in the global space. What global actually does is tell Python to use the reference to this name from the global space. When you're already in the global space, that's actually unnecessary.

Now that you know how they work... don't use them

People commonly recommend that you don't use global, as it's a messy way of circumventing scoping rules. A more robust solution is almost always to use a class, and this does fit your case.

If you made a class to hold your values and functions, then you'd just have class or instance attributes that are much easier to access than a series of loose global variables. You can also much clearer separate out what part of your code is concerned with what value/s.

Here's a simple example of how you might structure a class to take foo instead of using global:

class Example:
 def __init__(self, foo):
 self.foo = foo
 def bar(self):
 print self.foo
an_example = Example("Class based foo")
print an_example.foo
# Class based foo
an_example.bar()
# Class based foo

This is much simpler. Now foo is clearly paired with its instance of an_example. You can read it by calling self.foo and know that you have the right foo. It's also easy to change both inside and outside the instance. For example:

an_example.foo = "Easy to change"
print an_example.foo
# Easy to change
an_example.bar()
# Easy to change

If you wanted a value not to be tied to separate instances you can do that too (eg. if you wanted the class itself to have a counter), you can do that too with a class attribute:

class Example:
 foo = "I am foo"
 def bar(self):
 print Example.foo

This works very similarly, it can be accessed through the class name as well as the function:

an_example = Example()
an_example.bar()
# I am foo
print Example.foo
# I am foo

You can then update the class value itself, and the instance will reflect that:

Example.foo += " and I'm changeable."
print Example.foo
# I am foo and I'm changeable.
an_example.bar()
# I am foo and I'm changeable.

Data storage

Also on the data front, you have a lot of separate variables that seem like they should be a dictionary or list instead. Like your series of pub or p values. You should really look at how to organise your data neater as that will trickle down to much more readable code, like being able to loop over a list or dictionary rather than having to have a series of identical lines for each individual value.

• Xander Luciano: Now what if I would like to modify the global value of foo from within bar()? if I have foo="I am local" inside bar(), I call bar inside global, and then put print foo in the global space, will it print "I am global" or "I am local"?
• SuperBiasedMan: @ViperCode If I understand you right yes, the last example (with "I am redefined") shows how calling global modifies foo even when you're calling print foo outside of bar.
• Xander Luciano: Continuing on, is there anyway to specify you want to use a global variable inside a function? example: print global foo? I realized you should really just use different variable names to avoid confusion, but for sake of understanding better what would that result it?
• SuperBiasedMan: @ViperCode That example would result in a SyntaxError, as global var can only be used on a line by itself. Also global essentially brings the variable name into the function all over, so you can't partially use it and then use a different name elsewhere. You'll have to use different names, as you suggest.

Answer 2

A few obvious problems :

• global variables (SuperBiasedMan cover the topic pretty well, kudos!)

• code repetition : everything is done so many times that it will be a pain if you have to change anything. One can see in the commented code that you suffered a bit of it. Copy-pasting should not be your best friend.

• code organisation : everything is all over the place. It is impossible to tell which piece of datas are linked to the same thing.

To solve most of these issues, our friend will be functions and classes which help you split your code into logical pieces that can be easily reused.

The pieces of code below might need adjustments based on your need but should give you a few hints. Also the vocabulary I used might be wrong because I don't understand everything.

Joints

One thing I realized first is that you have 18 publishers which will be used to publish 18 positions. Also, we have 6 suscribers created with 6 callbacks updating 6 flags based on 6 (out of the 18 positions). My first thought was that I would be nice to a object to handle that kind of logic.

I've defined a new class like this :

class Joint():
 def __init__(self, name, position=0.0):
 self.name = name
 self.position = position
 self.publisher = rospy.Publisher('/' + name + '/command', Float64, latch=True)
 self.flag = 1
 def publish(self):
 self.publisher.publish(self.position) 
 def subscribe(self):
 def callback(msg):
 self.flag = 1 if abs(msg.current_pos - self.position) < delta else 0
 rospy.Subscriber('/' + name + '/state', JointState, callback)

On your Joint object, you can call subscribe() or publish() and it should just work. You might want to add other attributes like side (left/right), position (front, middle, back), index (1, 2, 3) but I had troubles as it was conflicting with the already created position attribute.

Then I thought : "do you know who is good at handling Joints? A robot!" and created another class which might or might not be useful to you.

class Robot():
 def __init__(self):
 self.moving = 1
 self.joints = []
 def send_command(self):
 for j in self.joints:
 j.publish()
 def subscribe(self):
 for j in self.joints:
 if j.is_middle: # to be defined
 j.subscribe
r = Robot()
for side in 'LR':
 for pos in 'FRM':
 for num in '123':
 r.joints.add(Joint(side + pos + num))

Functions to compute desired position

Another great tool to write reusable code is simple function. Sometimes, the hard part is to detect what is similar enough to be extracted out in functions. For most of your code, I had no idea what was going on so there is little I can do but by the end I noticed you were doing repeated calculations. At the end of the day, the 60 lines of computations were doing simple things : changing position based on some state. These computations, mostly repeated for left and right side are pretty similar each time, only a few constants are different based on the state we are in.

Thus, I wrote a function to retrieve the constants based on the state. Call it for the left side, then for the right side and then update positions accordingly.

def get_pos_for_state(state)
 if state == 1:
 return P_j1_s1, P_j2_s1, P_j3_s1
 elif state == 2:
 return P_j1_s2, P_j2_s2, P_j3_s2
 elif state == 3:
 return P_j1_s3, P_j2_s3, P_j3_s3
 assert False, "State must be 1, 2 or 3"
l1, l2, l3 = get_pos_for_state(LM_state)
r1, r2, r3 = get_pos_for_state(RM_state)
p_LM1 = Ra_L * l1
p_RM1 = - Ra_R * r1
p_LM2 = l2
p_RM2 = - r2
p_LM3 = l3
p_RM3 = - r3
p_RR1 = p_RF1 = - Ra_R * l1
p_LR1 = p_LF1 = Ra_L * r1
p_RR2 = p_RF2 = - l2
p_LR2 = p_LF2 = r2
p_RR3 = p_RF3 = - l3
p_LR3 = p_LF3 = r3

Even without knowing what this does, the symetry in the code is aesthetically pleasant and I hope it is correct :-).

I hope this will help you with your code... and your robot.