Exemple #1
0
 def __init__(self, leg_model, limb_controller, final_foot_pos, max_velocity, acceleration, z_plane=-0.67865):
     leg_logger.logger.info("New path.", path_name="TrapezoidalFootMove",
                 final_foot_pos=final_foot_pos, max_velocity=max_velocity,
                 acceleration=acceleration)
     
     self.model = leg_model
     self.controller = limb_controller
     self.final_foot_pos = final_foot_pos
     self.final_joint_angles = self.model.jointAnglesFromFootPos(final_foot_pos, shock_depth=0.0)
     self.max_vel = max_velocity
     self.vel = 0.0
     self.acc = acceleration
     self.z_plane = z_plane
     self.initial_foot_pos = self.model.footPosFromLegState([self.controller.getDesiredPosAngle(), 0])
     self.initial_joint_angles = self.controller.getDesiredPosAngle()
     self.target_angles = self.initial_joint_angles
     
     self.projected_initial = [self.initial_foot_pos[0], self.initial_foot_pos[1], z_plane]
     self.projected_final = [self.final_foot_pos[0], self.final_foot_pos[1], max(self.final_foot_pos[2],z_plane)]
     
     self.done = False
     
     self.LIFT = 1
     self.TRANSLATE = 2
     self.LOWER = 3
     self.state = self.LIFT
     
     self.path = TrapezoidalFootMove(self.model, self.controller, self.projected_initial, self.max_vel, self.acc)
     
     if self.initial_foot_pos[2] > self.z_plane:
         self.state = self.TRANSLATE
         self.path = TrapezoidalJointMove(self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc)
Exemple #2
0
    def update( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
        # Update model
        time_sources.global_time.updateTime(time)
        self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
        self.model.updateFootOnGround()

        # Init path. Do this after the first update.
        if self.path is None:
            self.path = Pause(self.model, controller, 1.0)
        
        if self.path.isDone():
            self.path = TrapezoidalFootMove(self.model, controller,
                    array(self.points[self.index][0:3]),
                    self.points[self.index][3], 0.4)
            self.index = (self.index+1)%len(self.points)

        # Evaluate path and joint control
        present_angles = self.model.getJointAngles()
        target_angles  = self.path.update()
        controller.update(present_angles, target_angles)

        control_lin_rates = [self.joint_controllers[i].update(target_angles[i], present_angles[i]) for i in range(3)]
        target_lin_rates = [c.getDesiredLengthRate() for c in self.joint_controllers]
        measured_lin_rates = [c.getLengthRate() for c in self.joint_controllers]

        if 0:
            s = ''
            m = self.pist_helpers[0].clay_pit_pos.mem
            for i in range(len(m)):
                if not i%8:
                    s += '%2.2f '%(m[i])
            print s

        commands  = [self.pist_helpers[i].update(control_lin_rates[i], measured_lin_rates[i]) for i in range(3)]
        #commands  = [self.pist_helpers[0].update(control_lin_rates[0], measured_lin_rates[0]),0,0]

        if 0:
            target_ang_rates = [c.desired_vel for c in self.joint_controllers]
            measured_ang_rates = [c.getAngleRate() for c in self.joint_controllers]
            s = 'Measured: '
            for m in measured_ang_rates:
                s += '%1.3f '%(m*180/pi)
            print s
            s = 'Target:   '
            for m in target_ang_rates:
                s += '%1.3f '%(m*180/pi)
            print s
            s = 'M Lin'
            for m in measured_lin_rates:
                s += '%1.3f '%m
            print s
            s = 'T Lin'
            for m in target_lin_rates:
                s += '%1.3f '%m
            print s
            self.pist_helpers[0].clay_pit_pos.printpit()

        return commands
Exemple #3
0
 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
Exemple #4
0
    def __init__(self,
                 leg_model,
                 limb_controller,
                 final_foot_pos,
                 max_velocity,
                 acceleration,
                 z_plane=-0.67865):
        leg_logger.logger.info("New path.",
                               path_name="TrapezoidalFootMove",
                               final_foot_pos=final_foot_pos,
                               max_velocity=max_velocity,
                               acceleration=acceleration)

        self.model = leg_model
        self.controller = limb_controller
        self.final_foot_pos = final_foot_pos
        self.final_joint_angles = self.model.jointAnglesFromFootPos(
            final_foot_pos, shock_depth=0.0)
        self.max_vel = max_velocity
        self.vel = 0.0
        self.acc = acceleration
        self.z_plane = z_plane
        self.initial_foot_pos = self.model.footPosFromLegState(
            [self.controller.getDesiredPosAngle(), 0])
        self.initial_joint_angles = self.controller.getDesiredPosAngle()
        self.target_angles = self.initial_joint_angles

        self.projected_initial = [
            self.initial_foot_pos[0], self.initial_foot_pos[1], z_plane
        ]
        self.projected_final = [
            self.final_foot_pos[0], self.final_foot_pos[1],
            max(self.final_foot_pos[2], z_plane)
        ]

        self.done = False

        self.LIFT = 1
        self.TRANSLATE = 2
        self.LOWER = 3
        self.state = self.LIFT

        self.path = TrapezoidalFootMove(self.model, self.controller,
                                        self.projected_initial, self.max_vel,
                                        self.acc)

        if self.initial_foot_pos[2] > self.z_plane:
            self.state = self.TRANSLATE
            self.path = TrapezoidalJointMove(
                self.model, self.controller,
                self.model.jointAnglesFromFootPos(self.projected_final),
                self.max_vel, self.acc)
Exemple #5
0
    def __init__(self, body_model, body_controller, leg_indices, delta_heights,
                 max_velocity, acceleration):
        logger.info("New path.",
                    path_name="TrapezoidalFeetLiftLower",
                    leg_indices=leg_indices,
                    delta_height=delta_heights,
                    max_velocity=max_velocity,
                    acceleration=acceleration)

        self.leg_indices = leg_indices
        self.model = body_model
        self.controller = body_controller

        self.final_joint_positions = self.controller.getTargetJointAngleMatrix(
        )

        self.foot_paths = [None for i in range(NUM_LEGS)]

        if isinstance(delta_heights, (int, long, float, complex)):
            delta_heights = [delta_heights] * 6

        for i in self.leg_indices:
            current_leg_pos = self.model.getLegs()[i].footPosFromLegState(
                [self.controller.getTargetJointAngleMatrix()[i],
                 0])  # self.model.getFootPositions()[i]
            desired_leg_pos = current_leg_pos + [0, 0, delta_heights[i]]

            self.foot_paths[i] = TrapezoidalFootMove(
                self.model.getLegs()[i],
                self.controller.getLimbControllers()[i], desired_leg_pos,
                max_velocity, acceleration)

        self.done = False
Exemple #6
0
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, 5.0)

    # Monitor leg_paths
    if path.isDone():
        if state == S_INIT:
            print "Move" * 1000
            path = TrapezoidalFootMove(model, controller,
                                       array([1.5, 0.0, -0.4]), 0.5, 0.5)
            state = S_MOVE_JOINT
        elif state == S_MOVE_JOINT:
            print "Done" * 1000
            state = S_DONE
            pass
        logger.info("State changed.", state=state)

    print "Foot:", model.getFootPos()
    # Evaluate path and joint control
    controller.update(model.getJointAngles(), path.update())

    # Send commands
    return controller.getLengthRateCommands()
Exemple #7
0
    def __init__(self, body_model, body_controller, final_height, max_velocity,
                 acceleration):
        logger.info("New path.",
                    path_name="TrapezoidalSitStand",
                    final_height=final_height,
                    max_velocity=max_velocity,
                    acceleration=acceleration)

        self.model = body_model
        self.controller = body_controller
        self.final_foot_positions = self.model.getFootPositions()
        self.feet_path = []

        for i in range(NUM_LEGS):
            self.final_foot_positions[i][2] = final_height

        for i in range(NUM_LEGS):
            self.feet_path = append(
                self.feet_path,
                TrapezoidalFootMove(self.model.getLegs()[i],
                                    self.controller.getLimbControllers()[i],
                                    self.final_foot_positions[i], max_velocity,
                                    acceleration))

        self.done = False
Exemple #8
0
 def update(self):
     if not self.isDone():
         self.target_angles = self.path.update()
         self.done = self.path.isDone()
     
     if self.isDone():
         if self.state == self.LIFT:
             self.state = self.TRANSLATE
             self.path = TrapezoidalJointMove(self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc)
             self.done = False
         elif self.state == self.TRANSLATE and self.final_foot_pos[2] < self.z_plane:
             self.state = self.LOWER
             self.path = TrapezoidalFootMove(self.model, self.controller, self.final_foot_pos, self.max_vel, self.acc)
             self.done = False
     
     
     return self.target_angles
Exemple #9
0
 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([2.0, 0.0, -0.2]),
                                                 0.4, 0.2)
     self.controller.update(self.model.getJointAngles(), self.initial_path.update())
     if self.initial_path.isDone():
         self.callback = self.pause
     return self.controller.getLengthRateCommands()
Exemple #10
0
    def update(self):
        if not self.isDone():
            self.target_angles = self.path.update()
            self.done = self.path.isDone()

        if self.isDone():
            if self.state == self.LIFT:
                self.state = self.TRANSLATE
                self.path = TrapezoidalJointMove(
                    self.model, self.controller,
                    self.model.jointAnglesFromFootPos(self.projected_final),
                    self.max_vel, self.acc)
                self.done = False
            elif self.state == self.TRANSLATE and self.final_foot_pos[
                    2] < self.z_plane:
                self.state = self.LOWER
                self.path = TrapezoidalFootMove(self.model, self.controller,
                                                self.final_foot_pos,
                                                self.max_vel, self.acc)
                self.done = False

        return self.target_angles
Exemple #11
0
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)

    # Monitor leg_paths
    if path.isDone():
        if state == S_MOVE3:
            path = TrapezoidalFootMove(model, controller,
                                       array([1.5, -0.6, -0.4]), 0.2, 0.1)
            state = S_MOVE1
        elif state == S_MOVE1:
            path = PutFootOnGround(model, controller, 0.05)
            state = S_LOWER
        elif state == S_LOWER:
            ground_level = model.getFootPos()[Z]
            path = TrapezoidalFootMove(model, controller,
                                       array([1.5, .6, ground_level]), 0.2,
                                       0.1)
            state = S_MOVE2
        elif state == S_MOVE2:
            path = TrapezoidalFootMove(model, controller,
                                       array([1.5, 0.6, -0.4]), 0.2, 0.1)
            state = S_MOVE3
        logger.info("State changed.", state=state)

    # Evaluate path and joint control
    controller.update(model.getJointAngles(), path.update())

    # Send commands
    return controller.getLengthRateCommands()
Exemple #12
0
 def __init__(self, body_model, body_controller, max_velocity=2.0, acceleration=1.0):
     leg_logger.logger.info("New path.",
                            path_name="GoToStandardHexagon",
                            max_velocity=max_velocity,
                            acceleration=acceleration)
     
     self.model = body_model
     self.controller = body_controller
     self.max_velocity = max_velocity
     self.acceleration = acceleration
     
     self.final_joint_positions = self.controller.getTargetJointAngleMatrix()
     self.foot_paths = [None for i in range(NUM_LEGS)]
     
     self.lifted = array([1.651 ,  0 ,  0])
     self.lowered = array([1.651 , 0 , -1.6764])
     
     self.NONE = 0
     self.LOWER_ALL = 1
     self.EVENS = 2
     self.LOWER_EVENS = 3
     self.ODDS = 4
     self.LOWER_ODDS = 5
     self.STAND = 6
     
     self.state = self.LOWER_ALL
     
     self.on_ground = [leg.isFootOnGround() for leg in self.model.getLegs()]
     self.in_place = [False] * 6
     
     # Put all 6 legs in place
     for i in range(NUM_LEGS):
         if not self.on_ground[i] and not self.in_place[i]:
             self.foot_paths[i] = TrapezoidalFootMove(self.model.getLegs()[i],
                                                      self.controller.getLimbControllers()[i],
                                                      self.lifted,
                                                      self.max_velocity, self.acceleration)
             self.in_place[i] = True
     
     
     self.phase_done = False
     self.done = False        
Exemple #13
0
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
    global points, path, index

    # 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)

    if path.isDone():
        path = TrapezoidalFootMove(model, controller, array(points[index]),
                                   0.5, 0.4)
        index = (index + 1) % len(points)

    # Evaluate path and joint control
    controller.update(model.getJointAngles(), path.update())

    # Send commands
    return controller.getLengthRateCommands()
Exemple #14
0
class SafeMove:
    """This is foot movement that combines up to two trapezoidal foot moves and up to one trapezoidal joint move to move the foot
        safely without fear of dragging on the ground.
        NOTE: The Z-plane is the highest horizontal plane that ensures a convex volume below it. 
        This is the boundary of where TrapFootMove is guaranteed to work, so we switch to TrapJointMove above it.
        It should be found via side-angle-side of the knee and calf links and the angle they make most folded up.
        We use only vertical motions below the Z-plane, as there is a high likelihood of hitting the ground.
    """
    def __init__(self, leg_model, limb_controller, final_foot_pos, max_velocity, acceleration, z_plane=-0.67865):
        leg_logger.logger.info("New path.", path_name="TrapezoidalFootMove",
                    final_foot_pos=final_foot_pos, max_velocity=max_velocity,
                    acceleration=acceleration)
        
        self.model = leg_model
        self.controller = limb_controller
        self.final_foot_pos = final_foot_pos
        self.final_joint_angles = self.model.jointAnglesFromFootPos(final_foot_pos, shock_depth=0.0)
        self.max_vel = max_velocity
        self.vel = 0.0
        self.acc = acceleration
        self.z_plane = z_plane
        self.initial_foot_pos = self.model.footPosFromLegState([self.controller.getDesiredPosAngle(), 0])
        self.initial_joint_angles = self.controller.getDesiredPosAngle()
        self.target_angles = self.initial_joint_angles
        
        self.projected_initial = [self.initial_foot_pos[0], self.initial_foot_pos[1], z_plane]
        self.projected_final = [self.final_foot_pos[0], self.final_foot_pos[1], max(self.final_foot_pos[2],z_plane)]
        
        self.done = False
        
        self.LIFT = 1
        self.TRANSLATE = 2
        self.LOWER = 3
        self.state = self.LIFT
        
        self.path = TrapezoidalFootMove(self.model, self.controller, self.projected_initial, self.max_vel, self.acc)
        
        if self.initial_foot_pos[2] > self.z_plane:
            self.state = self.TRANSLATE
            self.path = TrapezoidalJointMove(self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc)
    
    def isDone(self):
        return self.done

    def update(self):
        if not self.isDone():
            self.target_angles = self.path.update()
            self.done = self.path.isDone()
        
        if self.isDone():
            if self.state == self.LIFT:
                self.state = self.TRANSLATE
                self.path = TrapezoidalJointMove(self.model, self.controller, self.model.jointAnglesFromFootPos(self.projected_final), self.max_vel, self.acc)
                self.done = False
            elif self.state == self.TRANSLATE and self.final_foot_pos[2] < self.z_plane:
                self.state = self.LOWER
                self.path = TrapezoidalFootMove(self.model, self.controller, self.final_foot_pos, self.max_vel, self.acc)
                self.done = False
        
        
        return self.target_angles
Exemple #15
0
class SafeMove:
    """
    This is foot movement that combines up to two trapezoidal foot moves and up to one trapezoidal joint
    move to move the foot safely without fear of dragging on the ground.
    
    NOTE: The Z-plane is the highest horizontal plane that ensures a convex volume below it. 
    This is the boundary of where TrapFootMove is guaranteed to work, so we switch to TrapJointMove above it.
    It should be found via side-angle-side of the knee and calf links and the angle they make most folded up.
    We use only vertical motions below the Z-plane, as there is a high likelihood of hitting the ground.
    """
    def __init__(self,
                 leg_model,
                 limb_controller,
                 final_foot_pos,
                 max_velocity,
                 acceleration,
                 z_plane=-0.67865):
        leg_logger.logger.info("New path.",
                               path_name="TrapezoidalFootMove",
                               final_foot_pos=final_foot_pos,
                               max_velocity=max_velocity,
                               acceleration=acceleration)

        self.model = leg_model
        self.controller = limb_controller
        self.final_foot_pos = final_foot_pos
        self.final_joint_angles = self.model.jointAnglesFromFootPos(
            final_foot_pos, shock_depth=0.0)
        self.max_vel = max_velocity
        self.vel = 0.0
        self.acc = acceleration
        self.z_plane = z_plane
        self.initial_foot_pos = self.model.footPosFromLegState(
            [self.controller.getDesiredPosAngle(), 0])
        self.initial_joint_angles = self.controller.getDesiredPosAngle()
        self.target_angles = self.initial_joint_angles

        self.projected_initial = [
            self.initial_foot_pos[0], self.initial_foot_pos[1], z_plane
        ]
        self.projected_final = [
            self.final_foot_pos[0], self.final_foot_pos[1],
            max(self.final_foot_pos[2], z_plane)
        ]

        self.done = False

        self.LIFT = 1
        self.TRANSLATE = 2
        self.LOWER = 3
        self.state = self.LIFT

        self.path = TrapezoidalFootMove(self.model, self.controller,
                                        self.projected_initial, self.max_vel,
                                        self.acc)

        if self.initial_foot_pos[2] > self.z_plane:
            self.state = self.TRANSLATE
            self.path = TrapezoidalJointMove(
                self.model, self.controller,
                self.model.jointAnglesFromFootPos(self.projected_final),
                self.max_vel, self.acc)

    def isDone(self):
        return self.done

    def update(self):
        if not self.isDone():
            self.target_angles = self.path.update()
            self.done = self.path.isDone()

        if self.isDone():
            if self.state == self.LIFT:
                self.state = self.TRANSLATE
                self.path = TrapezoidalJointMove(
                    self.model, self.controller,
                    self.model.jointAnglesFromFootPos(self.projected_final),
                    self.max_vel, self.acc)
                self.done = False
            elif self.state == self.TRANSLATE and self.final_foot_pos[
                    2] < self.z_plane:
                self.state = self.LOWER
                self.path = TrapezoidalFootMove(self.model, self.controller,
                                                self.final_foot_pos,
                                                self.max_vel, self.acc)
                self.done = False

        return self.target_angles
Exemple #16
0
    def update(self):        
        active_paths = []
        active_index = []
        for i in range(NUM_LEGS):
            if self.foot_paths[i] is not None:
                active_index.append(i)
                active_paths.append(self.foot_paths[i])

        if self.phase_done:
            self.on_ground = [leg.isFootOnGround() for leg in self.model.getLegs()]
            
            if self.state == self.LOWER_ALL:
                for i in range(NUM_LEGS):
                    if not self.on_ground[i]:
                        self.foot_paths[i] = PutFootOnGround(
                            self.model.getLegs()[i],
                            self.controller.getLimbControllers()[i],
                            self.max_velocity / 5)
                        self.in_place[i] = True
                self.state = self.EVENS
                if all(self.in_place):
                    self.state = self.STAND 
                self.phase_done = False
            elif self.state == self.EVENS:
                for i in range(NUM_LEGS):
                    if not i % 2 and not self.in_place[i]:
                        self.foot_paths[i] = TrapezoidalFootMove(
                            self.model.getLegs()[i],
                            self.controller.getLimbControllers()[i],
                            self.lifted,
                            self.max_velocity, self.acceleration)
                self.state = self.LOWER_EVENS
                self.phase_done = False
            elif self.state == self.LOWER_EVENS:
                for i in range(NUM_LEGS):
                    if not self.on_ground[i]:
                        self.foot_paths[i] = PutFootOnGround(
                            self.model.getLegs()[i],
                            self.controller.getLimbControllers()[i],
                            self.max_velocity / 5)
                        self.in_place[i] = True
                self.state = self.ODDS
                self.phase_done = False
            elif self.state == self.ODDS:
                for i in range(NUM_LEGS):
                    if i % 2 and not self.in_place[i]:
                        self.foot_paths[i] = TrapezoidalFootMove(
                            self.model.getLegs()[i],
                            self.controller.getLimbControllers()[i],
                            self.lifted,
                            self.max_velocity, self.acceleration)
                self.state = self.LOWER_ODDS
                self.phase_done = False
            elif self.state == self.LOWER_ODDS:
                for i in range(NUM_LEGS):
                    if not self.on_ground[i]:
                        self.foot_paths[i] = PutFootOnGround(
                            self.model.getLegs()[i],
                            self.controller.getLimbControllers()[i],
                            self.max_velocity / 5)
                        self.in_place[i] = True
                self.state = self.STAND
                self.phase_done = False
            elif self.state == self.STAND:
                for i in range(NUM_LEGS):
                    self.foot_paths[i] = TrapezoidalFootMove(
                        self.model.getLegs()[i],
                        self.controller.getLimbControllers()[i],
                        self.lowered,
                        self.max_velocity, self.acceleration * .1)
                self.state = self.NONE
                self.phase_done = False
            elif self.state == self.NONE:
                self.done = True
                return self.final_joint_positions
        elif not self.phase_done:
            #logically and all of the isdone results from the joint move paths
            self.phase_done = all(map(lambda p: p.isDone(), active_paths))
        if not self.done:
            for i in active_index:
                self.final_joint_positions[i] = self.foot_paths[i].update()
            return self.final_joint_positions
Exemple #17
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
Exemple #18
0
class Gait(object):
    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]

    def update( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
        # Update model
        time_sources.global_time.updateTime(time)
        self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
        self.model.updateFootOnGround()

        # Init path. Do this after the first update.
        if self.path is None:
            self.path = Pause(self.model, controller, 1.0)
        
        if self.path.isDone():
            self.path = TrapezoidalFootMove(self.model, controller,
                    array(self.points[self.index][0:3]),
                    self.points[self.index][3], 0.4)
            self.index = (self.index+1)%len(self.points)

        # Evaluate path and joint control
        present_angles = self.model.getJointAngles()
        target_angles  = self.path.update()
        controller.update(present_angles, target_angles)

        control_lin_rates = [self.joint_controllers[i].update(target_angles[i], present_angles[i]) for i in range(3)]
        target_lin_rates = [c.getDesiredLengthRate() for c in self.joint_controllers]
        measured_lin_rates = [c.getLengthRate() for c in self.joint_controllers]

        if 0:
            s = ''
            m = self.pist_helpers[0].clay_pit_pos.mem
            for i in range(len(m)):
                if not i%8:
                    s += '%2.2f '%(m[i])
            print s

        commands  = [self.pist_helpers[i].update(control_lin_rates[i], measured_lin_rates[i]) for i in range(3)]
        #commands  = [self.pist_helpers[0].update(control_lin_rates[0], measured_lin_rates[0]),0,0]

        if 0:
            target_ang_rates = [c.desired_vel for c in self.joint_controllers]
            measured_ang_rates = [c.getAngleRate() for c in self.joint_controllers]
            s = 'Measured: '
            for m in measured_ang_rates:
                s += '%1.3f '%(m*180/pi)
            print s
            s = 'Target:   '
            for m in target_ang_rates:
                s += '%1.3f '%(m*180/pi)
            print s
            s = 'M Lin'
            for m in measured_lin_rates:
                s += '%1.3f '%m
            print s
            s = 'T Lin'
            for m in target_lin_rates:
                s += '%1.3f '%m
            print s
            self.pist_helpers[0].clay_pit_pos.printpit()

        return commands
    def gracefulExit(self):
        for i in range(3):
            self.pist_helpers[i].saveState()
Exemple #19
0
    def update(self,
               time,
               yaw,
               hip_pitch,
               knee_pitch,
               shock_depth,
               command=None):
        # Update model
        time_sources.global_time.updateTime(time)
        self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
        self.model.updateFootOnGround()

        # Init path. Do this after the first update.
        if self.path is None:
            self.path = Pause(self.model, controller, 1.0)

        if self.path.isDone():
            self.path = TrapezoidalFootMove(
                self.model, controller, array(self.points[self.index][0:3]),
                self.points[self.index][3], 0.4)
            self.index = (self.index + 1) % len(self.points)

        # Evaluate path and joint control
        present_angles = self.model.getJointAngles()
        target_angles = self.path.update()
        controller.update(present_angles, target_angles)

        control_lin_rates = [
            self.joint_controllers[i].update(target_angles[i],
                                             present_angles[i])
            for i in range(3)
        ]
        target_lin_rates = [
            c.getDesiredLengthRate() for c in self.joint_controllers
        ]
        measured_lin_rates = [
            c.getLengthRate() for c in self.joint_controllers
        ]

        if 0:
            s = ''
            m = self.pist_helpers[0].clay_pit_pos.mem
            for i in range(len(m)):
                if not i % 8:
                    s += '%2.2f ' % (m[i])
            print s

        commands = [
            self.pist_helpers[i].update(control_lin_rates[i],
                                        measured_lin_rates[i])
            for i in range(3)
        ]
        #commands  = [self.pist_helpers[0].update(control_lin_rates[0], measured_lin_rates[0]),0,0]

        if 0:
            target_ang_rates = [c.desired_vel for c in self.joint_controllers]
            measured_ang_rates = [
                c.getAngleRate() for c in self.joint_controllers
            ]
            s = 'Measured: '
            for m in measured_ang_rates:
                s += '%1.3f ' % (m * 180 / pi)
            print s
            s = 'Target:   '
            for m in target_ang_rates:
                s += '%1.3f ' % (m * 180 / pi)
            print s
            s = 'M Lin'
            for m in measured_lin_rates:
                s += '%1.3f ' % m
            print s
            s = 'T Lin'
            for m in target_lin_rates:
                s += '%1.3f ' % m
            print s
            self.pist_helpers[0].clay_pit_pos.printpit()

        return commands
Exemple #20
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
Exemple #21
0
class Gait(object):
    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]

    def update(self,
               time,
               yaw,
               hip_pitch,
               knee_pitch,
               shock_depth,
               command=None):
        # Update model
        time_sources.global_time.updateTime(time)
        self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth)
        self.model.updateFootOnGround()

        # Init path. Do this after the first update.
        if self.path is None:
            self.path = Pause(self.model, controller, 1.0)

        if self.path.isDone():
            self.path = TrapezoidalFootMove(
                self.model, controller, array(self.points[self.index][0:3]),
                self.points[self.index][3], 0.4)
            self.index = (self.index + 1) % len(self.points)

        # Evaluate path and joint control
        present_angles = self.model.getJointAngles()
        target_angles = self.path.update()
        controller.update(present_angles, target_angles)

        control_lin_rates = [
            self.joint_controllers[i].update(target_angles[i],
                                             present_angles[i])
            for i in range(3)
        ]
        target_lin_rates = [
            c.getDesiredLengthRate() for c in self.joint_controllers
        ]
        measured_lin_rates = [
            c.getLengthRate() for c in self.joint_controllers
        ]

        if 0:
            s = ''
            m = self.pist_helpers[0].clay_pit_pos.mem
            for i in range(len(m)):
                if not i % 8:
                    s += '%2.2f ' % (m[i])
            print s

        commands = [
            self.pist_helpers[i].update(control_lin_rates[i],
                                        measured_lin_rates[i])
            for i in range(3)
        ]
        #commands  = [self.pist_helpers[0].update(control_lin_rates[0], measured_lin_rates[0]),0,0]

        if 0:
            target_ang_rates = [c.desired_vel for c in self.joint_controllers]
            measured_ang_rates = [
                c.getAngleRate() for c in self.joint_controllers
            ]
            s = 'Measured: '
            for m in measured_ang_rates:
                s += '%1.3f ' % (m * 180 / pi)
            print s
            s = 'Target:   '
            for m in target_ang_rates:
                s += '%1.3f ' % (m * 180 / pi)
            print s
            s = 'M Lin'
            for m in measured_lin_rates:
                s += '%1.3f ' % m
            print s
            s = 'T Lin'
            for m in target_lin_rates:
                s += '%1.3f ' % m
            print s
            self.pist_helpers[0].clay_pit_pos.printpit()

        return commands

    def gracefulExit(self):
        for i in range(3):
            self.pist_helpers[i].saveState()