def zmp_check():
rospy.init_node('zmp_check')
rospy.loginfo("zmp_check node is ready")
nsw.pub_zmp = rospy.Publisher('zmpreal_out', zmp_real)
rospy.sleep(0.5)
contact_sub = rospy.Subscriber('/atlas/force_torque_sensors',
ForceTorqueSensors, get_foot_contact)
walking = rospy.Subscriber("zmp_out", walking_trajectory, get_walking_data)
nsw.out = zmp_real()
def zmp_check():
rospy.init_node('zmp_check')
rospy.loginfo( "zmp_check node is ready" )
nsw.pub_zmp = rospy.Publisher('zmpreal_out', zmp_real )
rospy.sleep(0.5)
contact_sub = rospy.Subscriber('/atlas/force_torque_sensors', ForceTorqueSensors, get_foot_contact)
# contact sensor filter:
a = [1,-3.180638548874721,3.861194348994217,-2.112155355110971,0.438265142261981]
b = [0.0004165992044065786,0.001666396817626,0.002499595226439,0.001666396817626,0.0004165992044065786]
ns.filter = contact_filter(b = b, a = a, use_internal_subscriber = False)
#walking = rospy.Subscriber("zmp_out", walking_trajectory, get_walking_data)
nsw.out = zmp_real()
class Nasmpace_l_leg:
pass
nl = Nasmpace_l_leg()
class namespace_walk:
pass
nsw = namespace_walk()
nsw.phase = 0
nsw.swing = Pos_and_Ori()
nsw.out = zmp_real()
##########################################################################################
# request from foot contact publisher
##########################################################################################
ns.zmpx_l = 0
ns.zmpy_l = 0
ns.zmpx_ld = 0
ns.zmpy_ld = 0
ns.zmpx_r = 0
ns.zmpy_r = 0
ns.zmpx_rd = 0
ns.zmpy_rd = 0
ns.zmp_double = [0.0, 0.0, 0.0, 0.0]
ns.num_of_samples = 0
class Nasmpace_r_leg:
pass
nr = Nasmpace_r_leg()
class Nasmpace_l_leg:
pass
nl = Nasmpace_l_leg()
ns.out = zmp_real()
ns.listener = tf.TransformListener()
ns.contact = contact_reflex()
ns.translation = 0
ns.rotation = 0
ns.zmpx_l = 0
ns.zmpy_l = 0
ns.zmpx_r = 0
ns.zmpy_r = 0
ns.Limit_R = 0
ns.Limit_L = 0
ns.Limit_Front = 0
ns.Limit_Back = 0
ns.errx = 0