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)