def __init__(self):
self.FT = Fitness()
rospy.init_node('Fit_function')
rospy.loginfo("Fitness node is ready")
self.listener = tf.TransformListener()
self.contact = contact_reflex()
self.state = 0
self._last_state = 0
self.force_z_r = 0
self.force_z_l = 0
self.contact_l = 0
self.contact_r = 0
contact_sub1 = rospy.Subscriber('/atlas/joint_states', JointState,
self.update_torque)
sub2 = rospy.Subscriber("zmpreal_out", zmp_real, self.update_zmp_real)
contact_sub = rospy.Subscriber('/atlas/force_torque_sensors',
ForceTorqueSensors,
self.update_zmp_state)
a = [
1, -3.180638548874721, 3.861194348994217, -2.112155355110971,
0.438265142261981
]
b = [
0.0004165992044065786, 0.001666396817626, 0.002499595226439,
0.001666396817626, 0.0004165992044065786
]
self.filter2 = contact_filter(b=b, a=a, use_internal_subscriber=False)
nr.force_x = 0
nr.force_y = 0
nr.force_z = 0
nr.t_x = 0
nr.t_y = 0
nr.t_z = 0
nl.force_x = 0
nl.force_y = 0
nl.force_z = 0
nl.t_x = 0
nl.t_y = 0
nl.t_z = 0
ns.response = [nl.force_x, nl.force_y, nl.force_z, nl.t_x, nl.t_y, nl.t_z, nr.force_x, nr.force_y, nr.force_z, nr.t_x, nr.t_y, nr.t_z]
ns.reflex=contact_reflex()
def get_l_foot_contact(msg):
#rospy.loginfo("wewewewewewewe")
nl.force_x = msg.wrench.force.x
nl.force_y = msg.wrench.force.y
nl.force_z = msg.wrench.force.z
nl.t_x = msg.wrench.torque.x
nl.t_y = msg.wrench.torque.y
nl.t_z = msg.wrench.torque.z
ns.num_of_samples_l = ns.num_of_samples_l + 1
#rospy.loginfo(nl.t_x)
def get_r_foot_contact(msg):
def __init__(self):
rospy.sleep(3)
self.contact = contact_reflex()
self.leg_force_z_r = 0
self.leg_force_z_l = 0
self.leg_force_x_r = 0
self.leg_force_x_l = 0
self.leg_force_y_r = 0
self.leg_force_y_l = 0
self.arm_force_z_r = 0
self.arm_force_z_l = 0
self.arm_force_x_r = 0
self.arm_force_x_l = 0
self.arm_force_y_r = 0
self.arm_force_y_l = 0
self.arm_force_r = 0
self.arm_force_l = 0
self.leg_force_r = 0
self.leg_force_l = 0
self.pitch_acc = 0
self.roll_acc = 0
self.yaw_acc = 0
self.count = 0
self.pitch_avg = 0
self.roll_avg = 0
self.yaw_avg = 0
self.last_start = 0
self.force_treshold = 800
self.time_treshold = 0.3
self.time_treshold_contact = 0.03
self.time_contact_r = 0.0
self.time_contact_l = 0.0
self.second_contact = ''
self.first_contact = ''
self.roll = 0
self.pitch = 0
self.yaw = 0
self.F_av = 0
self.Sigma = 0
self.stand_up_flag = 0
self.turned = 0
self.slop = 0
self.count_stand = 0
self.out = imu_contact()
self.contact_sub = rospy.Subscriber('/atlas/force_torque_sensors',
ForceTorqueSensors,
self.get_contact)
self.imu_check = rospy.Subscriber('/atlas/imu', Imu,
self.imu_manipulate)
a = [
1, -3.180638548874721, 3.861194348994217, -2.112155355110971,
0.438265142261981
]
b = [
0.0004165992044065786, 0.001666396817626, 0.002499595226439,
0.001666396817626, 0.0004165992044065786
]
self.filter = control_primitives.filter(b=b, a=a)
self.filter2 = contact_filter(b=b, a=a, use_internal_subscriber=False)
class Nasmpace_r_leg: pass nr = Nasmpace_r_leg() 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.listener = tf.TransformListener() ns.contact = contact_reflex() ns.zmpx_l = 0 ns.zmpy_l = 0 ns.zmpx_r = 0 ns.zmpy_r = 0 ns.errx = 0 ns.erry = 0 ns.translation =0 ns.rotation =0 nr.force_x = 0 nr.force_y = 0 nr.force_z = 0
def __init__(self):
rospy.sleep(3)
self.contact = contact_reflex()
self.leg_force_z_r = 0
self.leg_force_z_l = 0
self.leg_force_x_r = 0
self.leg_force_x_l = 0
self.leg_force_y_r = 0
self.leg_force_y_l = 0
self.arm_force_z_r = 0
self.arm_force_z_l = 0
self.arm_force_x_r = 0
self.arm_force_x_l = 0
self.arm_force_y_r = 0
self.arm_force_y_l = 0
self.arm_force_r = 0
self.arm_force_l = 0
self.leg_force_r = 0
self.leg_force_l = 0
self.pitch_acc = 0
self.roll_acc = 0
self.yaw_acc = 0
self.count = 0
self.pitch_avg = 0
self.roll_avg = 0
self.yaw_avg = 0
self.last_start = 0
self.force_treshold = 800
self.time_treshold = 0.3
self.time_treshold_contact = 0.03
self.time_contact_r = 0.0
self.time_contact_l = 0.0
self.second_contact = ''
self.first_contact = ''
self.roll = 0
self.pitch = 0
self.yaw = 0
self.F_av = 0
self.Sigma = 0
self.stand_up_flag = 0
self.turned = 0
self.slop = 0
self.count_stand = 0
self.out = imu_contact()
self.contact_sub = rospy.Subscriber('/atlas/force_torque_sensors', ForceTorqueSensors, self.get_contact)
self.imu_check = rospy.Subscriber('/atlas/imu',Imu, self.imu_manipulate)
a = [1,-3.180638548874721,3.861194348994217,-2.112155355110971,0.438265142261981]
b = [0.0004165992044065786,0.001666396817626,0.002499595226439,0.001666396817626,0.0004165992044065786]
self.filter = control_primitives.filter(b = b, a = a)
self.filter2 = contact_filter(b = b, a = a, use_internal_subscriber = False)