Пример #1
0
 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)
Пример #2
0
 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)
Пример #3
0
    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)
Пример #4
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)
Пример #5
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)