18马达，6腿机器人行走代码- Python / ROS -跟进

Question

我已经重新设计了我的机器人步行代码，从反馈，我收到了关于先前的问题，并希望听到一些反馈和指示。

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

Answer 1

没有简单的方法可以这么说，所以我会用困难的方式说:在一些方面，我更喜欢您的代码的原始版本，而不是喜欢当前的代码。

使用字典(或任何适当的数据结构)来存储信息是一个好主意，并且可能是将全局变量全部删除的关键。但是，将globals()添加到函数以便更新值会使事情变得比您最初试图处理的问题更糟。

尽管如此，我先前的回答的大部分评论在这里仍然是相关的，所以可以随意再看一看。

不管怎么说，这里有一些评论还没有说出来。

名称

Python有一个名为PEP 8的样式指南。它涵盖了许多方面，绝对值得一读。除其他外，它还涵盖了命名约定，特别是用CAPITAL_WITH_UNDERSCORES编写的常量更好。

rad是一个转换因子，没有改变的理由。RAD会给我更明确的解释。

leway的名称也可能得到改进。相应的评论写着error allowance，它甚至不对leway进行远程关闭。ERROR_ALLOWANCE可能会更好。我通常会调用像EPSILON_DELTA_POS这样的变量，因为它对应于这样一个事实，即任何小于此值的位置差都将被认为是0。

型

解决问题的最好方法之一是为正确的工作解决正确的工具。在计算机科学/编程中，正确的工具可能是正确的数据类型或数据结构。例如，您的flag_[LL]M[123]标志可能应该是布尔值，而不是int。即使它可以被认为是稍微相似的，它向任何阅读代码的人传达了更多的意义。

然后，这个代码：

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

可以写：

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

但到目前为止，这只是略有不同，真正的改进是可以写出来的：

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

然后，您的代码的其他部分可以改进:您可以用一个更清晰的if flag_LM1 == 1代替if flag_LM1 (尽管== 1仍然可以工作)。另外，flag_LM = flag_LM1*flag_LM2*flag_LM3可以重写flag_LM = flag_LM1 and flag_LM2 and flag_LM3。它看起来非常相似，并且做同样的事情，但它向任何阅读您的代码的人传达了更多的意义。

重排序条件

任何人都可能需要一段时间才能理解## here are the controls for the states of middle legs评论下发生的事情。

我花了一段时间才发现，这些条件不是分开的，我们可能在同一时间内有4个条件中的2个为真:第一个条件和最后一个条件(而且只有这一个条件)。

而且，需要一些思考才能意识到，这些条件是足够独立的，可以通过改变任何东西来重新排序。

出于这些原因，可能值得以类似的方式对条件进行分组，并尝试删除重复的逻辑。

我以以下代码结束：

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

现在，我可以看到，我们从一个类似的检查在两个第一条件，一个是左，一个只为右。

然后，只有当两个标志都是真的时候，我们才会发生一些事情，在这里，我们又有了一个左右对称。

Documentation

到处都是神奇的数字，使得代码很难理解。可能值得添加关于不同变量应该持有的内容以及不同值的含义的文档。特别是LM_state为0，1，2或3对我来说还是很神秘的。也许您可以尝试使用类似枚举的方法来给这些值取一个很酷的名字。

Answer 2

我想你有很多重复的地方。例如，您已经复制和粘贴了所有命令。如果您想使用eval，可以这样做：

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

但是，这使用eval。

相反，您可以创建一个包含所有发布者的pub字典。

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

使用上述的好处，可以调用所有的publish's在较少的行。

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

同样，这是相当丑陋的，如果p是一本字典，那就好多了。

您的程序大量使用globals，不要使用。如上所述，如果您使用字典，您的代码会更容易编写。目前，您正在强迫自己编写大量的样板，如果使用字典的话，就会容易得多。

大多数问题都来自于update_var。你很容易就能改变。

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