Exemplo n.º 1
0
 def __init__(self):
     self.file_out = file('offsets.txt', 'w+')
     self.vel_IIR = None
     self.pwm_val = 0
     self.last_pos = 0
     self.last_t = 0
     self.MOVE_THRESH = .01
     self.firstrun = True
     self.dof_list = []
     self.dof_list.append('hip_pitch_e')
     self.dof_list.append('hip_pitch_r')
     self.dof_list.append('knee_pitch_e')
     self.dof_list.append('knee_pitch_r')
     self.dof_list.append('hip_yaw_e')
     self.dof_list.append('hip_yaw_r')
     self.callback = self.moveToInitialPosition
     self.model = LegModel()
     self.controller = LimbController()
Exemplo n.º 2
0
    def __init__(self):
        self.model = LegModel()
        self.controller = LimbController()
        self.firstrun = True

        # Describe the actuator placements at each joint
        YAW_ACT = ActuatorMathHelper(bore_diameter=inch2meter * 2.5,
                                     rod_diameter=inch2meter * 1.125,
                                     act_retracted_len=inch2meter * 14.25,
                                     act_extended_len=inch2meter * 18.25,
                                     pivot1_dist_from_joint=inch2meter * 16.18,
                                     pivot2=(inch2meter * 2.32,
                                             inch2meter * 3.3),
                                     ang_offset=deg2rad * -30.0,
                                     system_pressure=psi2pascal * 2300,
                                     axis=(0, 0, -1))

        PITCH_ACT = ActuatorMathHelper(
            bore_diameter=inch2meter * 3.0,
            rod_diameter=inch2meter * 1.5,
            act_retracted_len=inch2meter * 20.25,
            act_extended_len=inch2meter * 30.25,
            pivot1_dist_from_joint=inch2meter * 8.96,
            pivot2=(inch2meter * 27.55, inch2meter * 8.03),
            ang_offset=deg2rad * -84.0,
            system_pressure=psi2pascal * 2300,
            axis=(0, -1, 0))

        KNEE_ACT = ActuatorMathHelper(bore_diameter=inch2meter * 2.5,
                                      rod_diameter=inch2meter * 1.25,
                                      act_retracted_len=inch2meter * 22.25,
                                      act_extended_len=inch2meter * 34.25,
                                      pivot1_dist_from_joint=inch2meter * 28.0,
                                      pivot2=(inch2meter * 4.3,
                                              inch2meter * 6.17),
                                      ang_offset=deg2rad * 61.84,
                                      system_pressure=psi2pascal * 2300,
                                      axis=(0, -1, 0))
        self.act_helpers = [YAW_ACT, PITCH_ACT, KNEE_ACT]

        yaw_pist = PistonController(YAW_ACT, 0.26, 0.28)
        pitch_pist = PistonController(PITCH_ACT, 0.27, 0.27)
        knee_pist = PistonController(KNEE_ACT, 0.22, 0.20)

        self.pist_helpers = [yaw_pist, pitch_pist, knee_pist]

        # Hackery for notch filter
        self.hip_yaw_commands = [
            0.0 for i in range(int((0.5 / 2.00) / (1 / 200.0)))
        ]
        self.hip_pitch_commands = [
            0.0 for i in range(int((0.5 / 6.30) / (1 / 200.0)))
        ]
        self.knee_commands = [
            0.0 for i in range(int((0.5 / 6.30) / (1 / 200.0)))
        ]
Exemplo n.º 3
0
 def __init__(self):
     self.file_out = file('offsets.txt', 'w+')
     self.vel_IIR = None
     self.pwm_val = 0
     self.last_pos = 0
     self.last_t = 0
     self.MOVE_THRESH = .01
     self.firstrun = True
     self.dof_list = []
     self.dof_list.append('hip_pitch_e')
     self.dof_list.append('hip_pitch_r')
     self.dof_list.append('knee_pitch_e')
     self.dof_list.append('knee_pitch_r')
     self.dof_list.append('hip_yaw_e')
     self.dof_list.append('hip_yaw_r')
     self.callback = self.moveToInitialPosition
     self.model = LegModel()
     self.controller = LimbController()
Exemplo n.º 4
0
class Gait:
    def __init__(self):
        self.file_out = file('offsets.txt', 'w+')
        self.vel_IIR = None
        self.pwm_val = 0
        self.last_pos = 0
        self.last_t = 0
        self.MOVE_THRESH = .01
        self.firstrun = True
        self.dof_list = []
        self.dof_list.append('hip_pitch_e')
        self.dof_list.append('hip_pitch_r')
        self.dof_list.append('knee_pitch_e')
        self.dof_list.append('knee_pitch_r')
        self.dof_list.append('hip_yaw_e')
        self.dof_list.append('hip_yaw_r')
        self.callback = self.moveToInitialPosition
        self.model = LegModel()
        self.controller = LimbController()
    def update( self, *args, **kwargs ):
        global_time.updateTime(args[0])
        return self.callback(*args, **kwargs)
    def moveToInitialPosition( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None ):
        self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
        if not hasattr(self, 'initial_path'):
            print "Starting move to initial position"
            self.initial_path = TrapezoidalFootMove(self.model, self.controller,\
                                       array([1.6, 0.0, -1.15]),\
                                       0.4, 0.2)
        self.controller.update(self.model.getJointAngles(), self.initial_path.update())
        if self.initial_path.isDone():
            self.callback = self.pause
        lr = self.controller.getLengthRateCommands()
        return lr
    def pause( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None ):
        if not hasattr( self, 'start_pause_time' ):
            print "Waiting..."
            self.start_pause_time = time
            #self.pause_path = Pause( self.model, self.controller, 3.0 )
        #self.controller.update(self.model.getJointAngles(), self.pause_path.update())
        if time - self.start_pause_time > 3.0:
            self.callback = self.discoverDeadband
            del self.start_pause_time
        return [0.0,0.0,0.0]
    def discoverDeadband( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None ):
        # Have we made it through all the joints?
        if not len(self.dof_list):
            # FIXME: Exit safely
            self.file_out.close()
            self.callback = self.pause
        if self.dof_list[0][:-2] == 'hip_pitch':
            pos = hip_pitch
            cmd_i = 1
        elif self.dof_list[0][:-2] == 'knee_pitch':
            pos = knee_pitch
            cmd_i = 2
        elif self.dof_list[0][:-2] == 'hip_yaw':
            pos = yaw
            cmd_i = 0
        else:
            raise
        if self.dof_list[0][-1] == 'e':
            sign = 1
        elif self.dof_list[0][-1] == 'r':
            sign = -1
        else:
            raise
        # Default command: command no movement on all valves
        # valves are yaw, pitch, knee, in that order
        # Scale depends on the mode of the underlying layer...
        # sometimes it expects a length rate in meters/sec,
        # sometimes it expects a command value between -255 and 255
        cmd = [0.0,0.0,0.0]
        # Is this our first tick on a new DOF?
        if self.firstrun:
            # Initialize state variables
            print "Initializing for %s"%self.dof_list[0]
            self.pwm_val = 0
            self.vel_IIR = LowPassFilter( gain=1.0, corner_frequency=.5 )
            self.firstrun = False
        else:
            # Calculate velocity, update filter
            dt = time-self.last_t
            dpos = pos - self.last_pos
            vel_est = dpos/dt
            self.vel_IIR.update(vel_est)
            # Take 10 seconds to ramp valve command
            self.pwm_val += dt*0.05
            cmd[cmd_i] = sign*self.pwm_val
        self.last_t = time
        self.last_pos = pos
        # Are we moving faster than our threshold?
        if sign*self.vel_IIR.getVal() > self.MOVE_THRESH:
            # If so, note the PWM value we had to apply and prepare to move the
            # next joint
            self.file_out.write( "%s: %f\n"%(self.dof_list[0],self.pwm_val) )
            print "%s: %f"%(self.dof_list[0],self.pwm_val)
            self.dof_list.pop(0)
            self.firstrun = True
            self.callback = self.pause
        return cmd
Exemplo n.º 5
0
from ControlsKit import time_sources, LegModel, LimbController
from ControlsKit.leg_paths import Pause, TrapezoidalFootMove, PutFootOnGround
from ControlsKit.math_utils import array, Z
from UI import logger

# Initialization
model = LegModel()
controller = LimbController()
path = None

S_MOVE1 = 0
S_LOWER = 1
S_MOVE2 = 2
S_MOVE3 = 3

state = S_MOVE3


# Body of control loop
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
    global path, state

    # Update model
    time_sources.global_time.updateTime(time)
    model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
    model.updateFootOnGround()

    # Init path. Do this after the first update.
    if path is None:
        path = Pause(model, controller, 1.0)
Exemplo n.º 6
0
    def __init__(self):
        self.model = LegModel()
        self.controller = LimbController()

        self.path = None
        self.index = 0
        self.points = []
        #self.points.append( (0.4,  0, -0.45, 0.2) )

        self.points.append((1.8, -1.00, -0.75, 0.40))
        self.points.append((1.8, -1.00, -1.68, 0.20))
        self.points.append((1.8, 1.00, -1.68, 0.40))
        self.points.append((1.8, 1.00, -0.75, 0.20))

        #self.points.append( (1.8, -1.00, -1.45, 0.40) )
        #self.points.append( (1.8, -1.00, -1.68, 0.70) )
        #self.points.append( (1.8,  1.00, -1.68, 0.70) )
        #self.points.append( (1.8,  0.00, -1.35, 0.70) )

        # Describe the actuator placements at each joint
        YAW_ACT = ActuatorMathHelper(bore_diameter=inch2meter * 2.5,
                                     rod_diameter=inch2meter * 1.125,
                                     act_retracted_len=inch2meter * 14.25,
                                     act_extended_len=inch2meter * 18.25,
                                     pivot1_dist_from_joint=inch2meter * 16.18,
                                     pivot2=(inch2meter * 2.32,
                                             inch2meter * 3.3),
                                     ang_offset=deg2rad * -30.0,
                                     system_pressure=psi2pascal * 2300,
                                     axis=(0, 0, -1))

        PITCH_ACT = ActuatorMathHelper(
            bore_diameter=inch2meter * 3.0,
            rod_diameter=inch2meter * 1.5,
            act_retracted_len=inch2meter * 20.25,
            act_extended_len=inch2meter * 30.25,
            pivot1_dist_from_joint=inch2meter * 8.96,
            pivot2=(inch2meter * 27.55, inch2meter * 8.03),
            ang_offset=deg2rad * -84.0,
            system_pressure=psi2pascal * 2300,
            axis=(0, -1, 0))

        KNEE_ACT = ActuatorMathHelper(bore_diameter=inch2meter * 2.5,
                                      rod_diameter=inch2meter * 1.25,
                                      act_retracted_len=inch2meter * 22.25,
                                      act_extended_len=inch2meter * 34.25,
                                      pivot1_dist_from_joint=inch2meter * 28.0,
                                      pivot2=(inch2meter * 4.3,
                                              inch2meter * 6.17),
                                      ang_offset=deg2rad * 61.84,
                                      system_pressure=psi2pascal * 2300,
                                      axis=(0, -1, 0))
        self.act_helpers = [YAW_ACT, PITCH_ACT, KNEE_ACT]

        yaw_joint = JointController(YAW_ACT,
                                    kp=1.0,
                                    ki=0.0,
                                    kd=0.0,
                                    kff=1.0,
                                    kfa=0.0,
                                    derivative_corner=10,
                                    backlash=deg2rad * 1.5)
        pitch_joint = JointController(PITCH_ACT,
                                      kp=1.0,
                                      ki=0.0,
                                      kd=0.0,
                                      kff=1.0,
                                      kfa=0.0,
                                      derivative_corner=10,
                                      backlash=deg2rad * 0.5)
        knee_joint = JointController(KNEE_ACT,
                                     kp=1.0,
                                     ki=0.0,
                                     kd=0.0,
                                     kff=1.0,
                                     kfa=0.0,
                                     derivative_corner=10,
                                     backlash=deg2rad * 0.75)

        self.joint_controllers = [yaw_joint, pitch_joint, knee_joint]

        #yaw_pist   = LearningPistonController(YAW_ACT  , 0.26, 0.28)
        yaw_pist = LearningPistonController('YAWPIST', YAW_ACT, 0.37, 0.37)
        pitch_pist = LearningPistonController('PITCHPIST', PITCH_ACT, 0.32,
                                              0.32)
        knee_pist = LearningPistonController('KNEEPIST', KNEE_ACT, 0.22, 0.20)

        self.pist_helpers = [yaw_pist, pitch_pist, knee_pist]
Exemplo n.º 7
0
class Gait:
    def __init__(self):
        self.file_out = file('offsets.txt', 'w+')
        self.vel_IIR = None
        self.pwm_val = 0
        self.last_pos = 0
        self.last_t = 0
        self.MOVE_THRESH = .01
        self.firstrun = True
        self.dof_list = []
        self.dof_list.append('hip_pitch_e')
        self.dof_list.append('hip_pitch_r')
        self.dof_list.append('knee_pitch_e')
        self.dof_list.append('knee_pitch_r')
        self.dof_list.append('hip_yaw_e')
        self.dof_list.append('hip_yaw_r')
        self.callback = self.moveToInitialPosition
        self.model = LegModel()
        self.controller = LimbController()

    def update(self, *args, **kwargs):
        global_time.updateTime(args[0])
        return self.callback(*args, **kwargs)

    def moveToInitialPosition(self,
                              time,
                              yaw,
                              hip_pitch,
                              knee_pitch,
                              shock_depth,
                              command=None):
        self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
        if not hasattr(self, 'initial_path'):
            print "Starting move to initial position"
            self.initial_path = TrapezoidalFootMove(self.model, self.controller,\
                                       array([1.6, 0.0, -1.15]),\
                                       0.4, 0.2)
        self.controller.update(self.model.getJointAngles(),
                               self.initial_path.update())
        if self.initial_path.isDone():
            self.callback = self.pause
        lr = self.controller.getLengthRateCommands()
        return lr

    def pause(self,
              time,
              yaw,
              hip_pitch,
              knee_pitch,
              shock_depth,
              command=None):
        if not hasattr(self, 'start_pause_time'):
            print "Waiting..."
            self.start_pause_time = time
            #self.pause_path = Pause( self.model, self.controller, 3.0 )
        #self.controller.update(self.model.getJointAngles(), self.pause_path.update())
        if time - self.start_pause_time > 3.0:
            self.callback = self.discoverDeadband
            del self.start_pause_time
        return [0.0, 0.0, 0.0]

    def discoverDeadband(self,
                         time,
                         yaw,
                         hip_pitch,
                         knee_pitch,
                         shock_depth,
                         command=None):
        # Have we made it through all the joints?
        if not len(self.dof_list):
            # FIXME: Exit safely
            self.file_out.close()
            self.callback = self.pause
        if self.dof_list[0][:-2] == 'hip_pitch':
            pos = hip_pitch
            cmd_i = 1
        elif self.dof_list[0][:-2] == 'knee_pitch':
            pos = knee_pitch
            cmd_i = 2
        elif self.dof_list[0][:-2] == 'hip_yaw':
            pos = yaw
            cmd_i = 0
        else:
            raise
        if self.dof_list[0][-1] == 'e':
            sign = 1
        elif self.dof_list[0][-1] == 'r':
            sign = -1
        else:
            raise
        # Default command: command no movement on all valves
        # valves are yaw, pitch, knee, in that order
        # Scale depends on the mode of the underlying layer...
        # sometimes it expects a length rate in meters/sec,
        # sometimes it expects a command value between -255 and 255
        cmd = [0.0, 0.0, 0.0]
        # Is this our first tick on a new DOF?
        if self.firstrun:
            # Initialize state variables
            print "Initializing for %s" % self.dof_list[0]
            self.pwm_val = 0
            self.vel_IIR = LowPassFilter(gain=1.0, corner_frequency=.5)
            self.firstrun = False
        else:
            # Calculate velocity, update filter
            dt = time - self.last_t
            dpos = pos - self.last_pos
            vel_est = dpos / dt
            self.vel_IIR.update(vel_est)
            # Take 10 seconds to ramp valve command
            self.pwm_val += dt * 0.05
            cmd[cmd_i] = sign * self.pwm_val
        self.last_t = time
        self.last_pos = pos
        # Are we moving faster than our threshold?
        if sign * self.vel_IIR.getVal() > self.MOVE_THRESH:
            # If so, note the PWM value we had to apply and prepare to move the
            # next joint
            self.file_out.write("%s: %f\n" % (self.dof_list[0], self.pwm_val))
            print "%s: %f" % (self.dof_list[0], self.pwm_val)
            self.dof_list.pop(0)
            self.firstrun = True
            self.callback = self.pause
        return cmd