def __init__(self, pathPlanner):
#StateMachine
StateMachine.__init__(self, State('Idle'))
self._bIsDone = False
self._StepQueue = deque([])
self._actualRobot = CD_ActualRobot(pathPlanner, self._StepQueue)
self._phantomRobot = CD_PhantomRobot(CD_PathPlanner(), Odometer(),
self._StepQueue)
# Add states
StateMachine.AddState(self, State('WaitingForPhantom'))
StateMachine.AddState(self, State('BothWalking'))
StateMachine.AddState(self, State('WaitingForActual'))
StateMachine.AddState(self, State('WindDown'))
# Add transitions
StateMachine.AddTransition(self, 'Idle', 'Start', 'WaitingForPhantom')
StateMachine.AddTransition(self, 'WaitingForPhantom', 'Both',
'BothWalking')
StateMachine.AddTransition(self, 'BothWalking', 'Actual',
'WaitingForActual')
StateMachine.AddTransition(self, 'WaitingForActual', 'WindingDown',
'WindDown')
StateMachine.AddTransition(self, 'WaitingForActual', 'Both',
'BothWalking')
StateMachine.AddTransition(self, 'WindDown', 'Done', 'Idle')
def __init__(self, localPathPlanner):
WalkingMode.__init__(self, localPathPlanner)
self.step_index_for_reset = 0
# Initialize atlas mode and atlas_sim_interface_command publishers
self.asi_command = rospy.Publisher(
'/atlas/atlas_sim_interface_command', AtlasSimInterfaceCommand,
None, False, True, None)
self._Odometer = Odometer()
# ##############################
# #self._StrategyForward = BDI_StrategyForward(self._Odometer)
# self._stepDataInit = BDI_Strategy(self._Odometer)
# self._StepQueue = StepQueue()
# self._SteppingStonesQueue = PathQueue()
# self._SteppingStonesPath = []
# self._setStep = True
# self._passedSteppingStones = False
# ##############################
self._BDI_StateMachine = BDI_StateMachine(self._Odometer)
#self._odom_position = Pose()
self._bDone = False
############################
#joint controller
self._cur_jnt = [0] * 28
robot_name = "atlas"
jnt_names = [
'back_lbz',
'back_mby',
'back_ubx',
'neck_ay', #3
'l_leg_uhz',
'l_leg_mhx',
'l_leg_lhy',
'l_leg_kny',
'l_leg_uay',
'l_leg_lax', #9
'r_leg_uhz',
'r_leg_mhx',
'r_leg_lhy',
'r_leg_kny',
'r_leg_uay',
'r_leg_lax', #15
'l_arm_usy',
'l_arm_shx',
'l_arm_ely',
'l_arm_elx',
'l_arm_uwy',
'l_arm_mwx', #21
'r_arm_usy',
'r_arm_shx',
'r_arm_ely',
'r_arm_elx',
'r_arm_uwy',
'r_arm_mwx'
] #27
self._JC = JointCommands_msg_handler(robot_name, jnt_names)
def __init__(self, iTf):
WalkingMode.__init__(self, DD_PathPlanner())
self._tf = iTf
# Initialize atlas atlas_sim_interface_command publisher
self.asi_command = rospy.Publisher(
'/atlas/atlas_sim_interface_command', AtlasSimInterfaceCommand,
None, False, True, None)
# for debug publish:
self._debug_pub_StepIndex = rospy.Publisher('debug_DD_StepIndex',
Int32)
self._Odometer = Odometer()
self._bDone = False
self._StepIndex = 1
self._command = 0
############################
#joint controller
self._cur_jnt = [0] * 28
robot_name = "atlas"
jnt_names = [
'back_lbz',
'back_mby',
'back_ubx',
'neck_ay', #3
'l_leg_uhz',
'l_leg_mhx',
'l_leg_lhy',
'l_leg_kny',
'l_leg_uay',
'l_leg_lax', #9
'r_leg_uhz',
'r_leg_mhx',
'r_leg_lhy',
'r_leg_kny',
'r_leg_uay',
'r_leg_lax', #15
'l_arm_usy',
'l_arm_shx',
'l_arm_ely',
'l_arm_elx',
'l_arm_uwy',
'l_arm_mwx', #21
'r_arm_usy',
'r_arm_shx',
'r_arm_ely',
'r_arm_elx',
'r_arm_uwy',
'r_arm_mwx'
] #27
self._JC = JointCommands_msg_handler(robot_name, jnt_names)