4
\$\begingroup\$

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()
\$\endgroup\$
  • \$\begingroup\$ Interesting question! I wish I could give it a try :-) \$\endgroup\$ – SylvainD Jan 17 '16 at 18:38
  • \$\begingroup\$ Follow-up question \$\endgroup\$ – 200_success Jan 18 '16 at 22:44
6
\$\begingroup\$

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.

\$\endgroup\$
  • \$\begingroup\$ 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"? \$\endgroup\$ – Xander Luciano Jan 18 '16 at 10:52
  • 1
    \$\begingroup\$ @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. \$\endgroup\$ – SuperBiasedMan Jan 18 '16 at 11:09
  • \$\begingroup\$ 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? \$\endgroup\$ – Xander Luciano Jan 19 '16 at 0:01
  • \$\begingroup\$ @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. \$\endgroup\$ – SuperBiasedMan Jan 19 '16 at 9:28
2
\$\begingroup\$

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.

\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.