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()
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()
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()
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
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()
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)
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
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()
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
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, 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()
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)
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
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 __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)
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)
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
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()
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:
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 __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
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)
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()
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
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()
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, 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()
def closeEvent(self, event):
# let the window close
logger.info("Bye")
event.accept()
self.exit()
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()
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)
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()
def closeEvent(self, event):
# let the window close
logger.info("Bye")
event.accept()
self.exit()