def __init__(self,robot_joints):
self._time = rospy.Time(0)
self._JointPos = cb_joints(robot_joints)
self._JointVel = cb_joints(robot_joints)
self._JointEff = cb_joints(robot_joints)
self._orientation = orientation()
self._angular_vel = [0,0,0]
a = [1,-3.180638548874721,3.861194348994217,-2.112155355110971,0.438265142261981]
b = [0.0004165992044065786,0.001666396817626,0.002499595226439,0.001666396817626,0.0004165992044065786]
self._vel_x_filter = control_primitives.filter(b,a)
self._vel_y_filter = control_primitives.filter(b,a)
self._vel_z_filter = control_primitives.filter(b,a)
def __init__(self, robot_joints):
self._time = rospy.Time(0)
self._JointPos = cb_joints(robot_joints)
self._JointVel = cb_joints(robot_joints)
self._JointEff = cb_joints(robot_joints)
self._orientation = orientation()
self._angular_vel = [0, 0, 0]
a = [
1, -3.180638548874721, 3.861194348994217, -2.112155355110971,
0.438265142261981
]
b = [
0.0004165992044065786, 0.001666396817626, 0.002499595226439,
0.001666396817626, 0.0004165992044065786
]
self._vel_x_filter = control_primitives.filter(b, a)
self._vel_y_filter = control_primitives.filter(b, a)
self._vel_z_filter = control_primitives.filter(b, a)
def __init__(self):
self.N = 2000
self.get_fit = 0
self.reset_buffer_torque()
self.zmp_real_m = 0
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)
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)
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)
