def update(time, leg_sensor_matrix, imu_orientation, imu_accelerations, imu_angular_rates, command): global path, state, leg_lift_height time_sources.global_time.updateTime(time) model.setSensorReadings(leg_sensor_matrix, imu_orientation, imu_angular_rates) target_angle_matrix = zeros((NUM_LEGS, LEG_DOF)) if path is None: path = BodyPause(model, controller, 1) state = STAND leg_lift_height = .2 if path.isDone(): if state == STAND: path = GoToStandardHexagon(model, controller) state = RAISE_FIRST_TRIPOD elif state == RAISE_FIRST_TRIPOD: path = TrapezoidalFeetLiftLower(model, controller, range(NUM_LEGS), [i*leg_lift_height for i in LIFT_TRIPOD_024], 2, 1) state = TURN_FIRST_TRIPOD elif state == TURN_FIRST_TRIPOD: path = RotateFeetAboutOrigin(model, controller, [0,2,4], TURN_ANGLE, 2, 1) state = RAISE_SECOND_TRIPOD elif state == RAISE_SECOND_TRIPOD: leg_lift_height = .4 path = TrapezoidalFeetLiftLower(model, controller, range(NUM_LEGS), [i*leg_lift_height for i in LIFT_TRIPOD_135], 2, 1) state = TURN_SECOND_TRIPOD elif state == TURN_SECOND_TRIPOD: path = RotateFeetAboutOrigin(model, controller, [1,3,5], TURN_ANGLE, 2, 1) state = FEET_ON_GROUND elif state == FEET_ON_GROUND: leg_lift_height = .2 path = TrapezoidalFeetLiftLower(model, controller, range(NUM_LEGS), [i*leg_lift_height for i in LIFT_TRIPOD_024], 2, 1) state=RESOLVE elif state == RESOLVE: path = RotateFeetAboutOrigin(model, controller, range(NUM_LEGS), -TURN_ANGLE, 2, 1) state = RAISE_FIRST_TRIPOD target_angle_matrix = path.update() #controller.setDesiredJointAngles(joint_angle_matrix) # Send commands return controller.update(model.getJointAngleMatrix(), target_angle_matrix)
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)
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())
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)
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)
def update(time, leg_sensor_matrix, imu_orientation, imu_accelerations, imu_angular_rates, command): global path, state, leg_lift_height time_sources.global_time.updateTime(time) model.setSensorReadings(leg_sensor_matrix, imu_orientation, imu_angular_rates) target_angle_matrix = zeros((NUM_LEGS, LEG_DOF)) if path is None: path = BodyPause(model, controller, 1) state = STAND leg_lift_height = .2 if path.isDone(): if state == STAND: path = GoToStandardHexagon(model, controller) state = RAISE_FIRST_TRIPOD elif state == RAISE_FIRST_TRIPOD: path = TrapezoidalFeetLiftLower( model, controller, range(NUM_LEGS), [i * leg_lift_height for i in LIFT_TRIPOD_024], 2, 1) state = TURN_FIRST_TRIPOD elif state == TURN_FIRST_TRIPOD: path = RotateFeetAboutOrigin(model, controller, [0, 2, 4], TURN_ANGLE, 2, 1) state = RAISE_SECOND_TRIPOD elif state == RAISE_SECOND_TRIPOD: leg_lift_height = .4 path = TrapezoidalFeetLiftLower( model, controller, range(NUM_LEGS), [i * leg_lift_height for i in LIFT_TRIPOD_135], 2, 1) state = TURN_SECOND_TRIPOD elif state == TURN_SECOND_TRIPOD: path = RotateFeetAboutOrigin(model, controller, [1, 3, 5], TURN_ANGLE, 2, 1) state = FEET_ON_GROUND elif state == FEET_ON_GROUND: leg_lift_height = .2 path = TrapezoidalFeetLiftLower( model, controller, range(NUM_LEGS), [i * leg_lift_height for i in LIFT_TRIPOD_024], 2, 1) state = RESOLVE elif state == RESOLVE: path = RotateFeetAboutOrigin(model, controller, range(NUM_LEGS), -TURN_ANGLE, 2, 1) state = RAISE_FIRST_TRIPOD target_angle_matrix = path.update() # controller.setDesiredJointAngles(joint_angle_matrix) # Send commands return controller.update(model.getJointAngleMatrix(), target_angle_matrix)