Пример #1
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()
Пример #2
0
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
    global path, state

    # Update leg 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)
        path.initial_angles[HP] = -0.6

    # Monitor model_path
    if path.isDone():
        if state == S_INIT:
            print "Move" * 1000
            path = MoveJoint(model, controller, joint_idx=KP, duration=3.0, direction=1, velocity=0.2)
            state = S_MOVE1
        elif state == S_MOVE1:
            print "Move" * 1000
            path = MoveJoint(model, controller, joint_idx=KP, duration=3.0, direction=-1, velocity=0.2)
            state = S_MOVE2
        elif state == S_MOVE2:
            print "Done" * 1000
            state = S_INIT
            pass
        logger.info("State changed.", state=state)
    
    # Evaluate path and joint control
    controller.update(model.getJointAngles(), path.update())

    # Send commands
    return controller.getLengthRateCommands()
Пример #3
0
def update(time, yaw, hip_pitch, knee_pitch, shock_depth, command=None):
    global path, state

    # Update leg 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)
        path.initial_angles[HP] = -0.6

    # Monitor model_path
    if path.isDone():
        if state == S_INIT:
            print "Move"*1000
            path = MoveJoint(model, controller, joint_idx=KP, duration=3.0, direction=1, velocity=0.2)
            state = S_MOVE1
        elif state == S_MOVE1:
            print "Move"*1000
            path = MoveJoint(model, controller, joint_idx=KP, duration=3.0, direction=-1, velocity=0.2)
            state = S_MOVE2
        elif state == S_MOVE2:
            print "Done"*1000
            state = S_INIT
            pass
        logger.info("State changed.", state=state)
    
    # Evaluate path and joint control
    controller.update(model.getJointAngles(), path.update())

    # Send commands
    return controller.getLengthRateCommands()
Пример #4
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
    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 type(delta_heights) == type(1.0):
            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
Пример #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()
Пример #7
0
 def __init__(self, body_model, body_controller, leg_index, delta_angle, max_velocity, acceleration):
     logger.info("New path.", path_name="RotateFootAboutOrigin",
                 delta_angle=delta_angle, max_velocity=max_velocity,
                 acceleration=acceleration)
     
     self.file_name = "rotate_foot_about_origin_data_leg%d.csv" % leg_index
     self.file = open(self.file_name, "w")
     
     self.body_model = body_model
     self.leg_index = leg_index
     self.leg_model = self.body_model.getLegs()[self.leg_index]
     self.body_controller = body_controller
     self.limb_controller = self.body_controller.getLimbControllers()[self.leg_index]
     self.target_foot_pos = self.leg_model.getFootPos()
     self.delta_angle = delta_angle
     self.max_ang_vel = max_velocity
     self.ang_vel = 0.0
     self.ang_acc = acceleration
     
     
     self.body_coord = self.body_model.transformLeg2Body(self.leg_index, self.leg_model.footPosFromLegState([self.limb_controller.getDesiredPosAngle(), 0]))#self.leg_model.getShockDepth()]))
     #self.body_coord = self.body_model.transformLeg2Body(self.leg_index,[2,0,-1])
     self.init_angle = arctan2(self.body_coord[1], self.body_coord[0])
     self.last_commanded_angle = self.init_angle
     self.target_angle = self.init_angle
     self.radius = norm([self.body_coord[0],self.body_coord[1]])
     self.init_height = self.body_coord[2]
     
     # Unit vector pointing towards the destination
     self.dir = sign(self.delta_angle)
     self.done = False
  
     self.sw = time_sources.StopWatch()
     self.sw.smoothStart(1)#self.accel_duration)
Пример #8
0
 def __init__(self, leg_model, limb_controller, final_foot_pos, max_velocity, acceleration):
     logger.info("New trajectory.", traj_name="TrapezoidalFootMove",
                 final_foot_pos=final_foot_pos, max_velocity=max_velocity,
                 acceleration=acceleration)
     
     self.model = leg_model
     self.controller = limb_controller
     # We want to start from the last commanded foot position,
     # not the last actual foot position.  However, this is not always
     # possible, since a position command may not have been given before this
     # point.
     last_target_ang_array = self.controller.getDesiredPosAngle()
     # If positions have been commanded to the controller
     if last_target_ang_array != None:
         # Base the starting position off that command
         self.target_foot_pos = self.model.footPosFromLegState(\
             (last_target_ang_array, 0.0) )
     else:
         # Base the starting position on the position of the model
         self.target_foot_pos = self.model.getFootPos()
     self.final_foot_pos = final_foot_pos
     self.max_vel = max_velocity
     self.vel = 0.0
     self.acc = acceleration
     
     self.start_on_ground = self.model.isFootOnGround()
     
     # Unit vector pointing towards the destination
     self.dir = self.getNormalizedRemaining()
     self.done = False
Пример #9
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 = TrapezoidalJointMove(
                model,
                controller,
                final_angles=[0, -0.59483773, 1.81300376],
                max_velocity=1,
                acceleration=.1
            )  # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
            state = S_MOVE_JOINT
        elif state == S_MOVE_JOINT:
            print "Done" * 1000
            state = S_DONE
            pass
        logger.info("State changed.", state=state)

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

    # Send commands
    return controller.getLengthRateCommands()
Пример #10
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
Пример #11
0
def update(time, leg_sensor_matrix, imu_orientation, imu_accelerations, imu_angular_rates, command=None):
    global path, state
    
    time_sources.global_time.updateTime(time)
    model.setSensorReadings(leg_sensor_matrix, imu_orientation, imu_angular_rates)
    
    target_angle_matrix = zeros((NUM_LEGS, LEG_DOF))
    
    # THIS IS WHERE WE CALL ON THE PATH TO DO MATH AND PRODUCE joint_angle_matrix (6x3 matrix)
    if path is None:
        path = BodyPause(model, controller, 1)
        state = ORIENT
        
    if path.isDone():
        if state == ORIENT:
            path = GoToStandardHexagon(model, controller)
            state = STAND
        elif state == STAND:
            path = TrapezoidalSitStand(model, controller, -1.6764, 1, .1)
            state = SIT
        elif state == SIT:
            path = TrapezoidalSitStand(model, controller, -0.6096, 1, .1)
            state = STAND
        elif state == 0:
            path = BodyPause(model, controller, 10)

        logger.info("State changed.", state=state)
    
    # Evaluate path and joint control
    target_angle_matrix  = path.update()
    # Send commands
    return controller.update(model.getJointAngleMatrix(), target_angle_matrix)
Пример #12
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 = TrapezoidalJointMove(model, controller, final_angles=[0, -0.59483773, 1.81300376],
                                        max_velocity=1, acceleration=.1) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
            state = S_MOVE_JOINT
        elif state == S_MOVE_JOINT:
            print "Done"*1000
            state = S_DONE
            pass
        logger.info("State changed.", state=state)
    
    # Evaluate path and joint control
    controller.update(model.getJointAngles(), path.update())

    # Send commands
    return controller.getLengthRateCommands()
Пример #13
0
    def __init__(self, cmdQueue, coreCmdQueue, cmdPipe):
        logger.info("Create MainWindow.")
        super(MainWindow, self).__init__()
        self.cmdQueue = cmdQueue
        self.coreCmdQueue = coreCmdQueue
        self.cmdPipe = cmdPipe
        self.isFillobjPropertyTree = False

        # load ui file
        uic.loadUi(UI_FILENAME, self)

        # action menus
        actionExit = self.findChild(QtGui.QAction, "actionExit")
        QtCore.QObject.connect(actionExit, QtCore.SIGNAL("triggered()"), self.exit)

        # action draw mode
        actionWireframe = self.findChild(QtGui.QAction, "actionWireframe")
        actionShading = self.findChild(QtGui.QAction, "actionShading")
        QtCore.QObject.connect(actionWireframe, QtCore.SIGNAL("triggered()"),
                               lambda: self.setViewMode(COMMAND.VIEWMODE_WIREFRAME))
        QtCore.QObject.connect(actionShading, QtCore.SIGNAL("triggered()"),
                               lambda: self.setViewMode(COMMAND.VIEWMODE_SHADING))

        # TabWidget - resource list
        self.resourceListWidget = self.findChild(QtGui.QTreeWidget, "resourceListWidget")
        self.resourceListWidget.itemDoubleClicked.connect(self.addResource)
        self.resourceListWidget.itemClicked.connect(self.selectResource)

        # TabWidget - object list
        self.objectList = self.findChild(QtGui.QListWidget, "objectList")
        self.objectList.itemClicked.connect(self.selectObject)
        self.objectList.itemActivated.connect(self.selectObject)
        self.objectList.itemDoubleClicked.connect(self.focusObject)

        # object property widget
        self.objPropertyTree = self.findChild(QtGui.QTreeWidget, "objPropertyTree")
        # hook editable event
        self.objPropertyTree.setEditTriggers(self.objPropertyTree.NoEditTriggers)
        # set object property events
        self.objPropertyTree.itemSelectionChanged.connect(self.checkEditable)
        self.objPropertyTree.itemClicked.connect(self.checkEditable)
        self.objPropertyTree.itemChanged.connect(self.objPropertyChanged)

        #
        # UIThread - Regist Recieve Signals
        #
        self.uiThread = UIThread(self.cmdQueue)
        self.connect(self.uiThread, QtCore.SIGNAL(get_command_name(COMMAND.CLOSE_UI)), self.exit)
        self.connect(self.uiThread, QtCore.SIGNAL(get_command_name(COMMAND.SEND_RESOURCE_LIST)), self.addResourceList)
        self.connect(self.uiThread, QtCore.SIGNAL(get_command_name(COMMAND.SEND_OBJECT_NAME)), self.addObjectName)
        self.connect(self.uiThread, QtCore.SIGNAL(get_command_name(COMMAND.SEND_OBJECT_DATA)), self.fillObjectData)
        self.uiThread.start()

        # wait a UI_RUN message, and send success message
        self.cmdPipe.RecvAndSend(COMMAND.UI_RUN, None, COMMAND.UI_RUN_OK, None)
        # request available mesh list
        self.coreCmdQueue.put(COMMAND.REQUEST_RESOURCE_LIST)
Пример #14
0
 def __init__(self, body_model, body_controller, duration):
     logger.info("New path.", path_name="BodyPause",
                            duration = duration)
     
     self.body = body_model
     self.body_controller = body_controller
     self.duration = duration
     self.feet_path = []
     
     for i in range (NUM_LEGS):
         self.feet_path = append(self.feet_path, Pause(self.body.getLegs()[i],
                                 self.body_controller.getLimbControllers()[i], self.duration))
         
     self.done = False
Пример #15
0
def update(time,
           leg_sensor_matrix,
           imu_orientation,
           imu_accelerations,
           imu_angular_rates,
           command=None):
    global path, state

    time_sources.global_time.updateTime(time)
    model.setSensorReadings(leg_sensor_matrix, imu_orientation,
                            imu_angular_rates)

    if path is None:
        path = BodyPause(model, controller, .1)
        state = ORIENT

    if path.isDone():
        if state == ORIENT:  # Put the feet in some reasonable location that they might have started from
            path = GoToStandardHexagon(model, controller)
            state = HALF_RAISE
        elif state == HALF_RAISE:  # Raise one tripod half the total travel
            path = LiftComposite(-lift_height / 2)
            state = HALF_TURN
        elif state == HALF_TURN:  # Rotate half the total rotation
            path = RotateComposite(-delta_angle / 2)
            state = RAISE_EVEN_TRIPOD
        elif state == RAISE_EVEN_TRIPOD:  # Raise a tripod
            path = LiftComposite(lift_height)
            state = CLOCKWISE
        elif state == CLOCKWISE:  # Rotate one direction
            path = RotateComposite(delta_angle)
            state = RAISE_ODD_TRIPOD
        elif state == RAISE_ODD_TRIPOD:  # Raise the other tripod
            path = LiftComposite(-lift_height)
            state = CCLOCKWISE
        elif state == CCLOCKWISE:  # Rotate the other direction
            path = RotateComposite(-delta_angle)
            state = RAISE_EVEN_TRIPOD
        elif state == PAUSE:  # Set state to PAUSE if you need to debug something
            path = BodyPause(model, controller, 10)

        logger.info("State changed.", state=state)

    # Evaluate path and joint control
    target_angle_matrix = path.update()

    # Send commands

    return controller.update(model.getJointAngleMatrix(), target_angle_matrix)
Пример #16
0
 def __init__(self, leg_model, limb_controller, velocity, accel_duration=0.1):
     logger.info("New trajectory.", traj_name="PutFootOnGround",
                 velocity=velocity, accel_duration=accel_duration)
     
     self.model = leg_model
     self.controller = limb_controller
     self.vel = velocity
     self.accel_duration = accel_duration
     
     self.done = self.model.isFootOnGround()
     self.target_foot_pos = self.model.footPosFromLegState(
         [self.controller.getDesiredPosAngle(),self.model.getShockDepth()])
     self.stop_watch = time_sources.StopWatch(active=False)
     if not self.done:
         self.stop_watch.smoothStart(self.accel_duration)
Пример #17
0
 def __init__(self, leg_model, limb_controller, velocity, accel_duration=0.1):
     logger.info("New trajectory.",
                 traj_name="PutFootOnGround",
                 velocity=velocity,
                 accel_duration=accel_duration)
     
     self.model = leg_model
     self.controller = limb_controller
     self.vel = velocity
     self.accel_duration = accel_duration
     
     self.done = self.model.isFootOnGround()
     self.target_foot_pos = self.model.footPosFromLegState(
         [self.controller.getDesiredPosAngle(), self.model.getShockDepth()])
     self.stop_watch = time_sources.StopWatch(active=False)
     if not self.done:
         self.stop_watch.smoothStart(self.accel_duration)
Пример #18
0
    def __init__(self, body_model, body_controller, duration):
        logger.info("New path.", path_name="BodyPause", duration=duration)

        self.body = body_model
        self.body_controller = body_controller
        self.duration = duration
        self.feet_path = []

        current_positions = self.body.getFootPositions()
        for i in range(NUM_LEGS):
            self.feet_path = append(
                self.feet_path,
                Pause(self.body.getLegs()[i],
                      self.body_controller.getLimbControllers()[i],
                      self.duration))

        self.done = False
Пример #19
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:
            path = SafeMove(
                model, controller, [2, 1, .5], 1, .1, 0
            )  # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
            state = S_MOVE_JOINT
        elif state == S_MOVE_JOINT:
            path = SafeMove(
                model, controller, [1.5, -.5, -1], 1, .1, 0
            )  # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
            state = 3
        elif state == 3:
            path = SafeMove(
                model, controller, [1.5, .5, -1], 1, .1, 0
            )  # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
            state = 4
        elif state == 4:
            path = SafeMove(
                model, controller, [2, -1, .5], 1, .1, 0
            )  # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
            state = S_INIT
        elif state == 5:
            state = S_DONE
            pass
        logger.info("State changed.", state=state)

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

    # Send commands
    return controller.getLengthRateCommands()
Пример #20
0
def update(time, leg_sensor_matrix, imu_orientation, imu_accelerations, imu_angular_rates, command=None):
    global path, state
    
    time_sources.global_time.updateTime(time)
    model.setSensorReadings(leg_sensor_matrix, imu_orientation, imu_angular_rates)
    
    if path is None:
        path = BodyPause(model, controller, .1)
        state = ORIENT
        
    if path.isDone():
        if state == ORIENT: # Put the feet in some reasonable location that they might have started from
            path = GoToStandardHexagon(model, controller)
            state = HALF_RAISE
        elif state == HALF_RAISE: # Raise one tripod half the total travel
            path = LiftComposite(-lift_height/2)
            state = HALF_TURN
        elif state == HALF_TURN: # Rotate half the total rotation
            path = RotateComposite(-delta_angle/2)
            state = RAISE_EVEN_TRIPOD      
        elif state == RAISE_EVEN_TRIPOD: # Raise a tripod
            path = LiftComposite(lift_height)
            state = CLOCKWISE
        elif state == CLOCKWISE: # Rotate one direction
            path = RotateComposite(delta_angle)
            state = RAISE_ODD_TRIPOD
        elif state == RAISE_ODD_TRIPOD: # Raise the other tripod
            path = LiftComposite(-lift_height)
            state = CCLOCKWISE
        elif state == CCLOCKWISE: # Rotate the other direction
            path = RotateComposite(-delta_angle)
            state = RAISE_EVEN_TRIPOD
        elif state == PAUSE: # Set state to PAUSE if you need to debug something
            path = BodyPause(model, controller, 10)

        logger.info("State changed.", state=state)
    
    # Evaluate path and joint control
    target_angle_matrix  = path.update()

    # Send commands

    return controller.update(model.getJointAngleMatrix(), target_angle_matrix)
Пример #21
0
def update(time, leg_sensor_matrix, imu_orientation, imu_accelerations, imu_angular_rates, command=None):
    global path, state
    
    time_sources.global_time.updateTime(time)
    model.setSensorReadings(leg_sensor_matrix, imu_orientation, imu_angular_rates)
    
    if path is None:
        path = BodyPause(model, controller, .1)
        state = ORIENT
    
    if path.isDone():
        if state == ORIENT:
            path = GoToStandardHexagon(model, controller)
            state = WALK
        elif state == WALK:
            path = TripodGait(model)
            state = DONE
        logger.info("State changed.", state=state)
    
    return controller.update(model.getJointAngleMatrix(), path.update())
Пример #22
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
Пример #23
0
    def __init__(self, body_model, body_controller, leg_index, delta_angle,
                 max_velocity, acceleration):
        logger.info("New path.",
                    path_name="RotateFootAboutOrigin",
                    delta_angle=delta_angle,
                    max_velocity=max_velocity,
                    acceleration=acceleration)

        self.file_name = "rotate_foot_about_origin_data_leg%d.csv" % leg_index
        self.file = open(self.file_name, "w")

        self.body_model = body_model
        self.leg_index = leg_index
        self.leg_model = self.body_model.getLegs()[self.leg_index]
        self.body_controller = body_controller
        self.limb_controller = self.body_controller.getLimbControllers()[
            self.leg_index]
        self.target_foot_pos = self.leg_model.getFootPos()
        self.delta_angle = delta_angle
        self.max_ang_vel = max_velocity
        self.ang_vel = 0.0
        self.ang_acc = acceleration

        self.body_coord = self.body_model.transformLeg2Body(
            self.leg_index,
            self.leg_model.footPosFromLegState(
                [self.limb_controller.getDesiredPosAngle(),
                 0]))  # self.leg_model.getShockDepth()]))
        #self.body_coord = self.body_model.transformLeg2Body(self.leg_index,[2,0,-1])
        self.init_angle = arctan2(self.body_coord[1], self.body_coord[0])
        self.last_commanded_angle = self.init_angle
        self.target_angle = self.init_angle
        self.radius = norm([self.body_coord[0], self.body_coord[1]])
        self.init_height = self.body_coord[2]

        # Unit vector pointing towards the destination
        self.dir = sign(self.delta_angle)
        self.done = False

        self.sw = time_sources.StopWatch()
        self.sw.smoothStart(1)  # self.accel_duration)
Пример #24
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()
Пример #25
0
 def __init__(self, body_model, body_controller, leg_indices, delta_angle, max_velocity, acceleration):
     logger.info("New path.", path_name="RotateFeetAboutOrigin",
                 delta_angle=delta_angle, leg_indices = leg_indices, max_velocity=max_velocity,
                 acceleration=acceleration)
     
     self.model = body_model
     self.controller = body_controller
     self.leg_indices = leg_indices
     self.final_joint_positions = self.model.getJointAngleMatrix()
     self.foot_paths = [None for i in range(NUM_LEGS)]
     self.delta_angle = delta_angle
     
     for i in self.leg_indices:
         self.foot_paths[i]=RotateFootAboutOrigin(
             self.model,
             self.controller,
             i,
             self.delta_angle,
             max_velocity,
             acceleration)
     
     self.done = False
Пример #26
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()
Пример #27
0
def update(time,
           leg_sensor_matrix,
           imu_orientation,
           imu_accelerations,
           imu_angular_rates,
           command=None):
    global path, state

    time_sources.global_time.updateTime(time)
    model.setSensorReadings(leg_sensor_matrix, imu_orientation,
                            imu_angular_rates)

    target_angle_matrix = zeros((NUM_LEGS, LEG_DOF))

    # THIS IS WHERE WE CALL ON THE PATH TO DO MATH AND PRODUCE joint_angle_matrix (6x3 matrix)
    if path is None:
        path = BodyPause(model, controller, 1)
        state = ORIENT

    if path.isDone():
        if state == ORIENT:
            path = GoToStandardHexagon(model, controller)
            state = STAND
        elif state == STAND:
            path = TrapezoidalSitStand(model, controller, -1.6764, 1, .1)
            state = SIT
        elif state == SIT:
            path = TrapezoidalSitStand(model, controller, -0.6096, 1, .1)
            state = STAND
        elif state == 0:
            path = BodyPause(model, controller, 10)

        logger.info("State changed.", state=state)

    # Evaluate path and joint control
    target_angle_matrix = path.update()
    # Send commands
    return controller.update(model.getJointAngleMatrix(), target_angle_matrix)
Пример #28
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:
            path = SafeMove(model, controller, [2, 1, .5], 1, .1, 0) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
            state = S_MOVE_JOINT
        elif state == S_MOVE_JOINT:
            path = SafeMove(model, controller, [1.5, -.5, -1], 1, .1, 0) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
            state = 3
        elif state == 3:
            path = SafeMove(model, controller, [1.5, .5, -1], 1, .1, 0) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
            state = 4
        elif state == 4:
            path = SafeMove(model, controller, [2, -1, .5], 1, .1, 0) # Simulation starts at angles=[-0.7504911, -0.99483773, 1.21300376]
            state = S_INIT
        elif state == 5:
            state = S_DONE
            pass
        logger.info("State changed.", state=state)

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

    # Send commands
    return controller.getLengthRateCommands()
Пример #29
0
 def closeEvent(self, event):
     # let the window close
     logger.info("Bye")
     event.accept()
     self.exit()
Пример #30
0
 def exit(self, *args):
     if args != () and args[0] is not None:
         logger.info(*args)
     self.coreCmdQueue.put(COMMAND.CLOSE_APP)
     self.close()
     sys.exit()
Пример #31
0
    def __init__(self, project_filename, cmdQueue, appCmdQueue, cmdPipe):
        logger.info("Create MainWindow.")
        super(MainWindow, self).__init__()
        self.project_filename = project_filename
        self.cmdQueue = cmdQueue
        self.appCmdQueue = appCmdQueue
        self.cmdPipe = cmdPipe
        self.selected_item = None
        self.selected_item_categoty = ''
        self.isFillAttributeTree = False

        # MessageThread
        self.message_thread = MessageThread(self.cmdQueue)
        self.message_thread.start()

        self.connect(self.message_thread,
                     QtCore.SIGNAL(get_command_name(COMMAND.CLOSE_UI)),
                     self.exit)

        # load ui file
        uic.loadUi(UI_FILENAME, self)

        # set windows title
        self.setWindowTitle(
            project_filename if project_filename else "Default Project")

        # exit
        actionExit = self.findChild(QtGui.QAction, "actionExit")
        QtCore.QObject.connect(actionExit, QtCore.SIGNAL("triggered()"),
                               self.exit)
        # project
        actionNewProject = self.findChild(QtGui.QAction, "actionNewProject")
        QtCore.QObject.connect(actionNewProject, QtCore.SIGNAL("triggered()"),
                               self.new_project)
        actionOpenProject = self.findChild(QtGui.QAction, "actionOpenProject")
        QtCore.QObject.connect(actionOpenProject, QtCore.SIGNAL("triggered()"),
                               self.open_project)
        actionSaveProject = self.findChild(QtGui.QAction, "actionSaveProject")
        QtCore.QObject.connect(actionSaveProject, QtCore.SIGNAL("triggered()"),
                               self.save_project)
        # scene
        actionNewScene = self.findChild(QtGui.QAction, "actionNewScene")
        QtCore.QObject.connect(actionNewScene, QtCore.SIGNAL("triggered()"),
                               self.new_scene)
        actionSaveScene = self.findChild(QtGui.QAction, "actionSaveScene")
        QtCore.QObject.connect(actionSaveScene, QtCore.SIGNAL("triggered()"),
                               self.save_scene)

        # action draw mode
        actionWireframe = self.findChild(QtGui.QAction, "actionWireframe")
        actionShading = self.findChild(QtGui.QAction, "actionShading")
        QtCore.QObject.connect(
            actionWireframe, QtCore.SIGNAL("triggered()"),
            lambda: self.setViewMode(COMMAND.VIEWMODE_WIREFRAME))
        QtCore.QObject.connect(
            actionShading, QtCore.SIGNAL("triggered()"),
            lambda: self.setViewMode(COMMAND.VIEWMODE_SHADING))

        # sort ui items
        self.connect(self.message_thread,
                     QtCore.SIGNAL(get_command_name(COMMAND.SORT_UI_ITEMS)),
                     self.sort_items)

        # Resource list
        self.resourceListWidget = self.findChild(QtGui.QTreeWidget,
                                                 "resourceListWidget")
        self.resource_menu = QMenu()
        self.resource_menu.addAction(self.tr("Load"), self.loadResource)
        self.resource_menu.addAction(self.tr("Open"), self.openResource)
        self.resource_menu.addAction(self.tr("Duplicate"),
                                     self.duplicateResource)
        self.resource_menu.addAction(self.tr("Save"), self.saveResource)
        self.resource_menu.addAction(self.tr("Delete"), self.deleteResource)
        self.resourceListWidget.setContextMenuPolicy(Qt.CustomContextMenu)
        self.resourceListWidget.customContextMenuRequested.connect(
            self.openResourceMenu)
        self.resourceListWidget.setSelectionMode(
            QtGui.QAbstractItemView.ExtendedSelection)
        self.resourceListWidget.setSortingEnabled(True)
        self.resourceListWidget.sortItems(0, 0)
        self.resourceListWidget.sortItems(1, 0)
        self.resourceListWidget.itemDoubleClicked.connect(self.loadResource)
        self.resourceListWidget.itemClicked.connect(self.selectResource)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.TRANS_RESOURCE_LIST)),
            self.addResourceList)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.TRANS_RESOURCE_INFO)),
            self.setResourceInfo)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.TRANS_RESOURCE_ATTRIBUTE)),
            self.fillResourceAttribute)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.DELETE_RESOURCE_INFO)),
            self.delete_resource_info)

        btn = self.findChild(QtGui.QPushButton, "btnOpenResource")
        btn.clicked.connect(self.openResource)

        btn = self.findChild(QtGui.QPushButton, "btnSaveResource")
        btn.clicked.connect(self.saveResource)

        btn = self.findChild(QtGui.QPushButton, "btnDeleteResource")
        btn.clicked.connect(self.deleteResource)

        btn = self.findChild(QtGui.QPushButton, "btnTest")
        btn.clicked.connect(self.test)

        # screen
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.TRANS_SCREEN_INFO)),
            self.setScreenInfo)
        self.spinWidth = self.findChild(QtGui.QSpinBox, "spinWidth")
        self.spinHeight = self.findChild(QtGui.QSpinBox, "spinHeight")
        self.checkFullScreen = self.findChild(QtGui.QCheckBox,
                                              "checkFullScreen")

        btn = self.findChild(QtGui.QPushButton, "btnChangeResolution")
        btn.clicked.connect(self.changeResolution)

        # render targets
        self.comboRenderTargets = self.findChild(QtGui.QComboBox,
                                                 "comboRenderTargets")
        self.comboRenderTargets.currentIndexChanged.connect(
            self.view_rendertarget)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.CLEAR_RENDERTARGET_LIST)),
            self.clearRenderTargetList)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.TRANS_RENDERTARGET_INFO)),
            self.addRenderTarget)

        # rendering type
        self.comboRenderingType = self.findChild(QtGui.QComboBox,
                                                 "comboRenderingType")
        self.comboRenderingType.currentIndexChanged.connect(
            self.set_rendering_type)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.TRANS_RENDERING_TYPE_LIST)),
            self.add_rendering_type)

        # anti aliasing
        self.comboAntiAliasing = self.findChild(QtGui.QComboBox,
                                                "comboAntiAliasing")
        self.comboAntiAliasing.currentIndexChanged.connect(
            self.set_anti_aliasing)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.TRANS_ANTIALIASING_LIST)),
            self.add_anti_aliasing)

        # game backend
        self.comboGameBackend = self.findChild(QtGui.QComboBox,
                                               "comboGameBackend")
        self.comboGameBackend.currentIndexChanged.connect(
            self.change_game_backend)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.TRANS_GAME_BACKEND_LIST)),
            self.add_game_backend)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.TRANS_GAME_BACKEND_INDEX)),
            self.set_game_backend_index)

        # Object list
        self.objectList = self.findChild(QtGui.QTreeWidget, "objectListWidget")
        self.object_menu = QMenu()
        self.object_menu.addAction(self.tr("Remove"), self.deleteObject)
        self.objectList.setContextMenuPolicy(Qt.CustomContextMenu)
        self.objectList.customContextMenuRequested.connect(self.openObjectMenu)
        self.objectList.setSelectionMode(
            QtGui.QAbstractItemView.ExtendedSelection)
        self.objectList.setSortingEnabled(True)
        self.objectList.sortItems(0, 0)
        self.objectList.sortItems(1, 0)
        self.objectList.itemClicked.connect(self.selectObject)
        self.objectList.itemActivated.connect(self.selectObject)
        self.objectList.itemDoubleClicked.connect(self.focusObject)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.DELETE_OBJECT_INFO)),
            self.deleteObjectInfo)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.TRANS_OBJECT_INFO)),
            self.addObjectInfo)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.TRANS_OBJECT_ATTRIBUTE)),
            self.fillObjectAttribute)
        self.connect(
            self.message_thread,
            QtCore.SIGNAL(get_command_name(COMMAND.CLEAR_OBJECT_LIST)),
            self.clearObjectList)

        btn = self.findChild(QtGui.QPushButton, "btnRemoveObject")
        btn.clicked.connect(self.deleteObject)

        # Object attribute tree
        self.attributeTree = self.findChild(QtGui.QTreeWidget, "attributeTree")
        self.attributeTree.setEditTriggers(
            self.attributeTree.NoEditTriggers)  # hook editable event
        self.attributeTree.itemSelectionChanged.connect(self.checkEditable)
        self.attributeTree.itemClicked.connect(self.checkEditable)
        self.attributeTree.itemChanged.connect(self.attributeChanged)

        # wait a UI_RUN message, and send success message
        if self.cmdPipe:
            self.cmdPipe.RecvAndSend(COMMAND.UI_RUN, None, COMMAND.UI_RUN_OK,
                                     None)
Пример #32
0
 def exit(self, *args):
     if args != () and args[0] is not None:
         logger.info(*args)
     self.appCmdQueue.put(COMMAND.CLOSE_APP)
     self.close()
     sys.exit()
Пример #33
0
 def closeEvent(self, event):
     # let the window close
     logger.info("Bye")
     event.accept()
     self.exit()