3
\$\begingroup\$

I have re-designed my robot walking code from feedback I received on previous question and would love to hear some feedback and pointers.

Main.py:

#!/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 States #Import created states.py

from std_msgs.msg import Float64
from dynamixel_msgs.msg import MotorStateList
from dynamixel_msgs.msg import JointState


## 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)


States.Update_var(globals()) #updates and assignes all global variables

global LM_state
global RM_state


#set initial states
LM_state = 1
RM_state = 3
States.LM_State1(globals())
States.RM_State3(globals())


leway = 4.00/rad  ## error allowance


def LM1_callback(msg):
    global flag_LM1
    flag_LM1 = 0  
    if abs(msg.current_pos-p_LM1)<leway:
            flag_LM1 = 1
def LM2_callback(msg):
    global flag_LM2
    flag_LM2 = 0  
    if abs(msg.current_pos-p_LM2)<leway:
            flag_LM2 = 1

def LM3_callback(msg):
    global flag_LM3
    flag_LM3 = 0 
    if abs(msg.current_pos-p_LM3)<leway:
            flag_LM3 = 1

def RM1_callback(msg):
    global flag_RM1
    flag_RM1 = 0  
    if abs(msg.current_pos-p_RM1)<leway:
            flag_RM1 = 1

def RM2_callback(msg):
    global flag_RM2
    if abs(msg.current_pos-p_RM2)<leway:
        flag_RM2 = 1


def RM3_callback(msg):
    global flag_RM3
    flag_RM3 = 0 
    if abs(msg.current_pos-p_RM3)<leway:
        #if msg.is_moving == False:
            flag_RM3 = 1

    global flag_LM1, flag_LM2, flag_LM3, flag_RM1, flag_RM2
    global LM_state
    global RM_state
    # = 1 if all joints are in the correct position
    flag_LM = flag_LM1*flag_LM2*flag_LM3  
    flag_RM = flag_RM1*flag_RM2*flag_RM3  

    ## 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


    if LM_state == 1:
        States.LM_State1(globals())             
    if LM_state == 2:
        States.LM_State2(globals())
    if LM_state == 3:
        States.LM_State3(globals())   
    if RM_state == 1:
        States.RM_State1(globals())
    if RM_state == 2:
        States.RM_State2(globals())
    if RM_state == 3:
        States.RM_State3(globals())

    ## send command to motors 
    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 Run():

    rospy.init_node('Run')

    ### subscribe
    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)   
    ## Loop
    rospy.spin()

if __name__ == '__main__':
     Run()

States.py:

Ra_L = 1
Ra_R = 1
rad = 180.000001/3.1415926
P_j1_s1 = -20/rad  ## state 1
P_j2_s1 = -65/rad
P_j3_s1 = -30/rad
P_j1_s2 = -20/rad  ## state 2
P_j2_s2 = -45/rad
P_j3_s2 = -20/rad
P_j1_s3 = 20/rad  ## state 3
P_j2_s3 = -45/rad
P_j3_s3 = -20/rad

def Update_var(dct):
    dct["rad"] = 180.000001 / 3.1415926
    dct["Ra_L"] = 1
    dct["Ra_R"] = 1
    dct["flag_LM1"] = 0
    dct["flag_LM2"] = 0
    dct["flag_LM3"] = 0
    dct["flag_RM1"] = 0
    dct["flag_RM2"] = 0
    dct["flag_RM3"] = 0
    dct["P_j1_s1"] = -20/rad
    dct["P_j2_s1"] = -65/rad
    dct["P_j3_s1"] = -30/rad
    dct["P_j1_s2"] = -20/rad
    dct["P_j2_s2"] = -45/rad
    dct["P_j3_s2"] = -20/rad
    dct["P_j1_s3"] = 20/rad
    dct["P_j2_s3"] = -45/rad
    dct["P_j3_s3"] = -20/rad
    #setstates LM = 1
    #setstates RM = 3 


def LM_State1(dct):
    dct["p_LM1"] = Ra_L*P_j1_s1
    dct["p_LM2"] = P_j2_s1
    dct["p_LM3"] = P_j3_s1
    dct["p_RR1"] = Ra_R*(-1)*P_j1_s1
    dct["p_RR2"] = (-1)*P_j2_s1
    dct["p_RR3"] = (-1)*P_j3_s1
    dct["p_RF1"] = Ra_R*(-1)*P_j1_s1
    dct["p_RF2"] = (-1)*P_j2_s1
    dct["p_RF3"] = (-1)*P_j3_s1   



def LM_State2(dct):

    dct["p_LM1"] = Ra_L*P_j1_s2
    dct["p_LM2"] = P_j2_s2
    dct["p_LM3"] = P_j3_s2
    dct["p_RR1"] = Ra_R*(-1)*P_j1_s2
    dct["p_RR2"] = (-1)*P_j2_s2
    dct["p_RR3"] = (-1)*P_j3_s2
    dct["p_RF1"] = Ra_R*(-1)*P_j1_s2
    dct["p_RF2"] = (-1)*P_j2_s2
    dct["p_RF3"] = (-1)*P_j3_s2 



def LM_State3(dct):

    dct["p_LM1"] = Ra_L*P_j1_s3
    dct["p_LM2"] = P_j2_s3
    dct["p_LM3"] = P_j3_s3
    dct["p_RR1"] = Ra_R*(-1)*P_j1_s3
    dct["p_RR2"] = (-1)*P_j2_s3
    dct["p_RR3"] = (-1)*P_j3_s3
    dct["p_RF1"] = Ra_R*(-1)*P_j1_s3
    dct["p_RF2"] = (-1)*P_j2_s3
    dct["p_RF3"] = (-1)*P_j3_s3 



def RM_State1(dct):

    dct["p_RM1"] = Ra_R*(-1)*P_j1_s1
    dct["p_RM2"] = (-1)*P_j2_s1
    dct["p_RM3"] = (-1)*P_j3_s1
    dct["p_LR1"] = Ra_L*P_j1_s1
    dct["p_LR2"] = P_j2_s1
    dct["p_LR3"] = P_j3_s1
    dct["p_LF1"] = Ra_L*P_j1_s1
    dct["p_LF2"] = P_j2_s1
    dct["p_LF3"] = P_j3_s1



def RM_State2(dct):

    dct["p_RM1"] = Ra_R*(-1)*P_j1_s2
    dct["p_RM2"] = (-1)*P_j2_s2
    dct["p_RM3"] = (-1)*P_j3_s2
    dct["p_LR1"] = Ra_L*P_j1_s2
    dct["p_LR2"] = P_j2_s2
    dct["p_LR3"] = P_j3_s2
    dct["p_LF1"] = Ra_L*P_j1_s2
    dct["p_LF2"] = P_j2_s2
    dct["p_LF3"] = P_j3_s2




def RM_State3(dct):

    dct["p_RM1"] = Ra_R*(-1)*P_j1_s3
    dct["p_RM2"] = (-1)*P_j2_s3
    dct["p_RM3"] = (-1)*P_j3_s3
    dct["p_LR1"] = Ra_L*P_j1_s3
    dct["p_LR2"] = P_j2_s3
    dct["p_LR3"] = P_j3_s3
    dct["p_LF1"] = Ra_L*P_j1_s3
    dct["p_LF2"] = P_j2_s3
    dct["p_LF3"] = P_j3_s3

def Turn_left(dct):
    dct["Ra_L"] = 0.25

def Turn_right(dct):
    dct["Ra_R"] = 0.25

def Stop_turn(dct):
    dct["Ra_R"] = 1
    dct["Ra_L"] = 1

    #NameError: global name 'rad' is not defined
\$\endgroup\$

2 Answers 2

4
\$\begingroup\$

There is no easy way to say it so I'll say it the hard way : on a few aspects, I liked the original version of your code more than I like the current one.

Using a dictionary (or any kind of appropriate data structure) to store the information is a good idea and might be the key to remove all the global variable all over the place. However, feeding globals() to functions so that it updates values make things kind of worse that the issue you are initially trying to deal with.

This being said, most of the comments of my previous answer are still relevant here so feel free to have another look.

Anyway, here are a few comments that haven't been said yet.

Names

Python has a style guide called PEP 8. It covers many aspects and is definitely worth a read. Among other things, it covers naming convention and especially constants are better written in CAPITAL_WITH_UNDERSCORES.

rad is a conversion factor and has no reason to change. RAD would me much more explicit.

The name for leway could probably be improved too. The corresponding comment says error allowance which is not even remotely closed to leway. ERROR_ALLOWANCE might be better. I'd usually call such a variable like EPSILON_DELTA_POS because it corresponds to the fact that any position difference smaller than this will be considered to be 0.

Type

One of the best way to solve issues is to solve the right tool for the right job. In computer science/programming, the right tool might be the right data type or data structure. For instance, your flag_[LL]M[123] flags should probably be booleans rather than int. Even if it can be considered as slightly similar, it conveys a lot more meaning to anyone who reads the code.

Then, this code :

def LM1_callback(msg):
    global flag_LM1
    flag_LM1 = 0  
    if abs(msg.current_pos-p_LM1)<leway:
            flag_LM1 = 1

can be written :

def LM1_callback(msg):
    global flag_LM1
    flag_LM1 = False
    if abs(msg.current_pos-p_LM1)<leway:
            flag_LM1 = True

but this is only slightly different so far, the real improvement it that can be written:

def LM1_callback(msg):
    global flag_LM1
    flag_LM1 = abs(msg.current_pos-p_LM1)<leway

Then, other parts of your code can be improved : you can replace the if flag_LM1 == 1 by a much clearer if flag_LM1 (even though the == 1 would still work). Also, flag_LM = flag_LM1*flag_LM2*flag_LM3 can be rewritten flag_LM = flag_LM1 and flag_LM2 and flag_LM3. It looks pretty similar and does the same thing but it conveys a lot more meaning to anyone reading your code.

Reordering conditions

It might take a while to anyone to understand what's happening under the ## here are the controls for the states of middle legs comment.

It took me a while to see that the conditions were not disjoints, we might have 2 of the 4 conditions to be true in the same time : the first and the last (and only these one).

Also, it takes some thinking to realise that the conditions are independent enough to be reordered with changing anything.

For these reasons, it might be worth grouping conditions working in a similar way and trying to remove duplicating logic.

I ended with the following code :

if flag_LM and LM_state==1:
    LM_state = 2
    flag_LM = False
if flag_RM and RM_state==1:
    RM_state = 2
    flag_RM = False

if flag_LM and flag_RM:
    if LM_state==2 and RM_state==3:
        LM_state, RM_state = 3, 1
        flag_LM, flag_RM = False, False
    elif RM_state==2 and LM_state==3:
        RM_state, LM_state = 3, 1
        flag_RM, flag_LM = False, False

Now, I can see that we start with a similar check in the 2 first conditions, one is for left only, one for right only.

Then, we have things that will happen only if both flags are True and here again we have a symmetry between left and right.

Documentation

Having magic numbers all over the place makes the code hard to understand. It might be worth adding documentation about what the different variables are supposed to hold and the meaning for the different values. In particular LM_state being 0, 1, 2 or 3 is still very cryptic to me. Maybe you could try to use something like enumerations to give these values a cool name.

\$\endgroup\$
1
\$\begingroup\$

I think that you have a lot of repetition. For example, you have copied and pasted all of the commands. If you wanted to use eval you could do:

for i in ["LF1", "LF2"]:
    eval('pub_{0} = rospy.Publisher("/{0}/command", Float64, latch=True)'.format(i))

However, this uses eval.
Instead, you could make a pub dictionary, that contains all the publishers.

pub = {
    i: rospy.Publisher("/{}/command".format(i), Float64, latch=True)
    for i in ["LF1", "LF2"]
}

Using the above has the benefit, of being able to call all the publish's in less lines.

local = locals()
for key, value in pub.items():
    value.publish(local['p_' + key])

Again this is quite ugly, and would be much nicer if p were a dictionary.


Your program heavily uses globals, don't. As highlighted above your code is easier to write if you use dictionary's. You are currently forcing yourself to write lots of boilerplate that would be much easier if you used dictionary's.

Most of the problems arise from update_var. Which you can easily change.

def update_var():
    ra = {
        'L': 1,
        'R': 1
    }
    flag = {
        'LM1': 0,
        'LM2': 0,
        'LM3': 0,
        'RM1': 0,
        'RM2': 0,
        'RM3': 0
    }
    p = {
        ...
    }
    return rad, ra, flag, p

# Main.py
rad, ra, flag, p = update_var()
\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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