Esempio n. 1
0
class DD_WalkingMode(WalkingMode):
    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)

    def Initialize(self, parameters):
        WalkingMode.Initialize(self, parameters)
        self._LPP.Initialize()
        self._bRobotIsStatic = True

        self._BDIswitch_client = rospy.ServiceProxy('C25/BDIswitch', C25BDI)
        state = Int32()
        state.data = 1
        resp_switched_to_BDI_odom = self._BDIswitch_client(state)
        print "Using BDI odom"
        # rospy.wait_for_service('foot_placement_path') # used for clone
        # self._foot_placement_client = rospy.ServiceProxy('foot_placement_path', FootPlacement_Service)
        rospy.wait_for_service('foot_placement')
        self._foot_placement_client = rospy.ServiceProxy(
            'foot_placement', FootPlacement_Service)
        # Subscribers:
        self._Subscribers["Odometry"] = rospy.Subscriber(
            '/C25/publish', C25C0_ROP, self._odom_cb)
        self._Subscribers["ASI_State"] = rospy.Subscriber(
            '/atlas/atlas_sim_interface_state', AtlasSimInterfaceState,
            self.asi_state_cb)
        self._Subscribers["IMU"] = rospy.Subscriber('/atlas/imu', Imu,
                                                    self._get_imu)
        self._Subscribers["JointStates"] = rospy.Subscriber(
            '/atlas/joint_states', JointState, self._get_joints)
        rospy.sleep(0.3)
        self._k_effort = [0] * 28
        self._k_effort[3] = 255  # k_effort[0:4] = 4*[255]
        # k_effort[16:28] = 12*[255]
        self._JC.set_k_eff(self._k_effort)
        self._JC.set_all_pos(self._cur_jnt)
        self._JC.send_command()

        self._RequestFootPlacements()
        self._bDone = False
        self._bIsSwaying = False
        self._StepIndex = 1
        self._command = 0
        #self._bRobotIsStatic = False
        self._GetOrientationDelta0Values(
        )  # Orientation difference between BDI odom and Global

        # Put robot into stand position
        stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
                                         None, None, None, None,
                                         self._k_effort)
        self.asi_command.publish(stand)
        rospy.sleep(0.3)

    def StartWalking(self):
        self._bDone = False

    def Walk(self):
        WalkingMode.Walk(self)
        command = self.GetCommand()
        #print ("Walk ",command)
        self.asi_command.publish(command)
        #print(command)
        self._WalkingModeStateMachine.PerformTransition("Go")
        self._StepIndex = self._StepIndex + 1

    def EmergencyStop(self):
        WalkingMode.Stop(self)

    def IsDone(self):
        return self._bDone

    def GetCommand(self):
        command = AtlasSimInterfaceCommand()
        command.behavior = AtlasSimInterfaceCommand.WALK
        # command.k_effort = [0] * 28
        # k_effort = [0] * 28
        # k_effort[3] = 255 # k_effort[0:4] = 4*[255]
        # # k_effort[16:28] = 12*[255]
        step_queue = self._LPP.GetNextStep()
        if (0 == step_queue):
            command = 0
        else:
            for i in range(4):
                command.walk_params.step_queue[i] = copy.deepcopy(
                    step_queue[i])
                command.walk_params.step_queue[
                    i].step_index = self._StepIndex + i
                command.walk_params.step_queue[i].duration = 0.63
                #print("GetCommand",command.walk_params.step_queue)
                command.walk_params.step_queue[
                    i] = self._TransforFromGlobalToBDI(
                        command.walk_params.step_queue[i], i)
        return command

    def HandleStateMsg(self, state):
        command = 0
        if ("Idle" == self._WalkingModeStateMachine.GetCurrentState().Name):
            self._Odometer.SetPosition(state.pos_est.position.x,
                                       state.pos_est.position.y)
        elif ("Wait" == self._WalkingModeStateMachine.GetCurrentState().Name):
            self._Odometer.SetPosition(state.pos_est.position.x,
                                       state.pos_est.position.y)
            print("Odometer Updated")
            #print(2)
            self._WalkingModeStateMachine.PerformTransition("Go")
        elif ("Walking" == self._WalkingModeStateMachine.GetCurrentState().Name
              ):
            #print(3)
            if (DD_PathPlannerEnum.Active == self._LPP.State):
                command = self.GetCommand()
            elif (DD_PathPlannerEnum.Waiting == self._LPP.State):
                self._RequestFootPlacements()
        elif ("Done" == self._WalkingModeStateMachine.GetCurrentState().Name):
            #print(4)
            self._bDone = True
        else:
            raise Exception("QS_WalkingModeStateMachine::Bad State Name")

        return command

    def _RequestFootPlacements(self):
        print("Request Foot Placements")
        # Perform a service request from FP
        try:
            # Handle preemption?
            # if received a "End of mission" sort of message from FP
            start_pose, other_foot_pose = self._GetStartingFootPose()
            resp = self._foot_placement_client(0, start_pose, other_foot_pose)
            if [] == resp.foot_placement_path:  # 1 == resp.done:
                self._WalkingModeStateMachine.PerformTransition("Finished")
            else:
                listSteps = []
                for desired in resp.foot_placement_path:
                    #print("Desired: ",desired)
                    command = AtlasSimInterfaceCommand()
                    step = command.step_params.desired_step
                    step.foot_index = desired.foot_index
                    step.swing_height = desired.clearance_height
                    step.pose.position.x = desired.pose.position.x
                    step.pose.position.y = desired.pose.position.y
                    step.pose.position.z = desired.pose.position.z
                    Q = quaternion_from_euler(desired.pose.ang_euler.x,
                                              desired.pose.ang_euler.y,
                                              desired.pose.ang_euler.z)
                    step.pose.orientation.x = Q[0]
                    step.pose.orientation.y = Q[1]
                    step.pose.orientation.z = Q[2]
                    step.pose.orientation.w = Q[3]
                    listSteps.append(step)
                self._LPP.SetPath(listSteps)
                #print("listSteps: ",listSteps)
        except rospy.ServiceException, e:
            print "Foot Placement Service call failed: %s" % e
Esempio n. 2
0
class AP_WalkingMode(WalkingMode):
    def __init__(self, iTf):
        self._LPP = AP_PathPlanner()
        WalkingMode.__init__(self, self._LPP)
        self._tf = iTf
        self._StepIndex = 1
        # Initialize atlas atlas_sim_interface_command publisher
        self.asi_command = rospy.Publisher(
            '/atlas/atlas_sim_interface_command', AtlasSimInterfaceCommand,
            None, False, True, None)

        self._Odometer = Odometer()
        self._bDone = False
        self._bIsSwaying = False
        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)

    def Initialize(self, parameters):
        WalkingMode.Initialize(self, parameters)
        self._LPP.Initialize()
        self._command = 0
        self._bRobotIsStatic = True
        self._bGotStaticPose = False
        self._BDI_Static_pose = Pose()
        self._started_to_walk = False
        self._target_pose = None
        self._StepIndex = 1
        self._isDynamic = False
        self._stepWidth = 0.25  # Width of stride
        self._theta_max = 0.30  # max turning angle per step
        self._x_length_max = 0.25  # [meters] max step length (radius =~ 0.42 [m])
        self._y_length_max = 0.15  # [meters]

        # parameters to tune (see also 'Motion' task parameters):
        self._err_rot = 0.018  #0.10 # [rad]
        self._err_trans = 0.02  # 0.1 # [meters]

        ## USING task parameters:
        if ((None != parameters) and ('Motion' in parameters)):
            DesiredMotion = parameters['Motion']
            if "Dynamic" == DesiredMotion:  # Dynamic parameters
                self._isDynamic = True
                self._stepWidth = 0.2  #0.3 # Width of stride
                self._theta_max = 0.15  #0.35 # max turning angle per step
                self._x_length_max = 0.02  #0.25 # [meters] max step length (radius =~ 0.42 [m])
                self._y_length_max = 0.15  #0.2 # [meters]
            if "Dynamic10" == DesiredMotion:  # Dynamic parameters
                self._isDynamic = True
                self._stepWidth = 0.2  #0.3 # Width of stride
                self._theta_max = 0.15  #0.35 # max turning angle per step
                self._x_length_max = 0.10  #0.25 # [meters] max step length (radius =~ 0.42 [m])
                self._y_length_max = 0.10  #0.2 # [meters]
        else:  # Quasi-Static parameters (default)
            self._isDynamic = False
            self._stepWidth = 0.25  # Width of stride
            self._theta_max = 0.30  # max turning angle per step
            self._x_length_max = 0.25  # [meters] max step length (radius =~ 0.42 [m])
            self._y_length_max = 0.15  # [meters]
        self._R = self._stepWidth / 2  # Radius of turn, turn in place

        # parameter 'Object' uses service to get delta alignment to target object
        if ((None != parameters) and ('Object' in parameters)):
            self._DesiredObject = parameters['Object']
        else:
            self._DesiredObject = "delta"
        # parameter to turn in place and move (translate):
        self._delta_yaw = 0.0
        self._delta_trans = Point()
        self._delta_trans.x = 0.0
        self._delta_trans.y = 0.0
        self._delta_trans.z = 0.0
        # 'turn_in_place_Yaw' uses the parameter of the Yaw angle to turn in place
        if ((None != parameters) and ('turn_in_place_Yaw' in parameters)):
            self._target_pose = "Rotate_Translate_delta"
            self._delta_yaw = float(parameters['turn_in_place_Yaw'])
        # 'Xmovement' uses the parameter to translate x meters (+=fwd/-=bwd) from the starting place of the robot
        if ((None != parameters) and ('Xmovement' in parameters)):
            self._target_pose = "Rotate_Translate_delta"
            self._delta_trans.x = float(parameters['Xmovement'])
        # 'Ymovement' uses the parameter to translate y meters (+=left/-=right) from the starting place of the robot
        if ((None != parameters) and ('Ymovement' in parameters)):
            self._target_pose = "Rotate_Translate_delta"
            self._delta_trans.y = float(parameters['Ymovement'])
        if None == self._target_pose:
            rospy.wait_for_service('C23/C66')
            self._foot_placement_client = rospy.ServiceProxy(
                'C23/C66', C23_orient)  # object recognition service
        # rospy.wait_for_service('foot_aline_pose')
        # self._foot_placement_client = rospy.ServiceProxy('foot_aline_pose', C23_orient) # clone_service
        self._BDIswitch_client = rospy.ServiceProxy('C25/BDIswitch', C25BDI)
        state = Int32()
        state.data = 1
        resp_switched_to_BDI_odom = self._BDIswitch_client(state)
        print "Using BDI odom"
        # Subscribers:
        #self._Subscribers["Odometry"] = rospy.Subscriber('/ground_truth_odom',Odometry,self._odom_cb)
        self._Subscribers["ASI_State"] = rospy.Subscriber(
            '/atlas/atlas_sim_interface_state', AtlasSimInterfaceState,
            self.asi_state_cb)
        self._Subscribers["IMU"] = rospy.Subscriber('/atlas/imu', Imu,
                                                    self._get_imu)
        self._Subscribers["JointStates"] = rospy.Subscriber(
            '/atlas/joint_states', JointState, self._get_joints)
        rospy.sleep(0.3)

        self._RequestTargetPose(self._DesiredObject)
        self._k_effort = [0] * 28
        self._k_effort[3] = 255
        # self._k_effort[0:4] = 4*[255]
        # self._k_effort[16:28] = 12*[255]
        self._JC.set_k_eff(self._k_effort)
        self._JC.set_all_pos(self._cur_jnt)
        self._JC.send_command()
        self._bDone = False
        self._bIsSwaying = False
        #self._bRobotIsStatic = False
        #self._GetOrientationDelta0Values() # Orientation difference between BDI odom and Global

        # Put robot into stand position
        stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
                                         None, None, None, None,
                                         self._k_effort)
        self.asi_command.publish(stand)
        rospy.sleep(0.3)

    def StartWalking(self):
        self._bDone = False

    def Walk(self):
        WalkingMode.Walk(self)
        #self._command = self.GetCommand(self._BDI_state)
        if self._isDynamic:
            self._command = self.GetCommandDynamic()
            self._StepIndex = self._StepIndex + 1
        else:
            self._command = self.GetCommandStatic(self._BDI_state)
        if (0 != self._command):
            self.asi_command.publish(self._command)
            self._bIsSwaying = True
        #print(command)
        self._WalkingModeStateMachine.PerformTransition("Go")

    def EmergencyStop(self):
        WalkingMode.Stop(self)

    def Stop(self):
        WalkingMode.Stop(self)

    def IsDone(self):
        return self._bDone

    def IsReady(self):
        return True

    def GetCommandStatic(self, state):
        command = AtlasSimInterfaceCommand()
        command.behavior = AtlasSimInterfaceCommand.STEP
        #give user control over neck, back_z and arms
        command.k_effort = self._k_effort
        command.step_params.desired_step = self._LPP.GetNextStaticStep()
        if (0 != command.step_params.desired_step):
            # Not sure why such a magic number
            command.step_params.desired_step.duration = 0.63
            # Apparently this next line is a must
            command.step_params.desired_step.step_index = 1
            #command.step_params.desired_step = self._TransforFromGlobalToBDI(command.step_params.desired_step,state)
        else:
            command = 0
        return command

    def GetCommandDynamic(self):
        command = AtlasSimInterfaceCommand()
        command.behavior = AtlasSimInterfaceCommand.WALK
        # command.k_effort = [0] * 28
        # k_effort = [0] * 28
        # k_effort[3] = 255 # k_effort[0:4] = 4*[255]
        # # k_effort[16:28] = 12*[255]
        command.k_effort = self._k_effort
        step_queue = self._LPP.GetNextDynamicStep()
        if (0 == step_queue):
            command = 0
        else:
            for i in range(4):
                command.walk_params.step_queue[i] = copy.deepcopy(
                    step_queue[i])
                command.walk_params.step_queue[
                    i].step_index = self._StepIndex + i
                command.walk_params.step_queue[i].duration = 0.63
                #print("GetCommand",command.walk_params.step_queue)
                #command.walk_params.step_queue[i] = self._TransforFromGlobalToBDI(command.walk_params.step_queue[i],i)
        return command

    def HandleStateMsg(self, state):
        command = 0
        if ("Idle" == self._WalkingModeStateMachine.GetCurrentState().Name):
            self._Odometer.SetPosition(state.pos_est.position.x,
                                       state.pos_est.position.y)
        elif ("Wait" == self._WalkingModeStateMachine.GetCurrentState().Name):
            self._Odometer.SetPosition(state.pos_est.position.x,
                                       state.pos_est.position.y)
            print("Odometer Updated")
            #print(2)
            self._WalkingModeStateMachine.PerformTransition("Go")
        elif ("Walking" == self._WalkingModeStateMachine.GetCurrentState().Name
              ):
            #print(3)
            if (AP_PathPlannerEnum.Active == self._LPP.State):
                if self._isDynamic:
                    command = self.GetCommandDynamic()
                else:
                    command = self.GetCommandStatic(state)
            elif (AP_PathPlannerEnum.Waiting == self._LPP.State
                  ):  # or (AP_PathPlannerEnum.Empty == self._LPP.State):
                self._RequestTargetPose(self._DesiredObject)
        elif ("Done" == self._WalkingModeStateMachine.GetCurrentState().Name):
            #print(4)
            self._bDone = True
        else:
            raise Exception("AL_WalkingModeStateMachine::Bad State Name")

        return command

    def _RequestTargetPose(self, desired_object):
        # Perform a service request from FP
        try:
            # Handle preemption?
            # if received a "End of mission" sort of message from FP
            #start_pose,other_foot_pose = self._GetStartingFootPose()
            if None == self._target_pose:
                print("Request Target Pose to ", desired_object)
                self._target_pose = self._foot_placement_client(
                    desired_object)  # "target: 'Firehose'") #
                print("Got from C23/C66 service the following Pose:",
                      self._target_pose)
                delta_yaw, delta_trans = self._GetDeltaToObject(
                    desired_object, self._target_pose)
            elif "Rotate_Translate_delta" == self._target_pose and not self._started_to_walk:
                delta_yaw = self._delta_yaw
                delta_trans = self._delta_trans
            else:
                delta_yaw = 0.0
                delta_trans = Point()
                delta_trans.x = 0.0
                delta_trans.y = 0.0
                delta_trans.z = 0.0

            start_position = copy.copy(self._BDI_Static_pose.position)
            start_orientation = euler_from_quaternion([
                self._BDI_Static_pose.orientation.x,
                self._BDI_Static_pose.orientation.y,
                self._BDI_Static_pose.orientation.z,
                self._BDI_Static_pose.orientation.w
            ])
            self._foot_placement_path = []
            if math.fabs(delta_yaw) > self._err_rot:
                self._foot_placement_path = self._foot_placement_path + self._GetRotationDeltaFP_Path(
                    delta_yaw, start_position, start_orientation)
                self._started_to_walk = True
            if self._DistanceXY(delta_trans) > self._err_trans:
                self._foot_placement_path = self._foot_placement_path + self._GetTranslationDeltaFP_Path(
                    delta_yaw, delta_trans, start_position, start_orientation)
                self._started_to_walk = True

            #listSteps = []
            # if (math.fabs(delta_yaw) <= self._err_rot) and (self._DistanceXY(delta_trans) <= self._err_trans): # finished task
            if [] == self._foot_placement_path and self._started_to_walk:  # 1 == resp.done:
                self._WalkingModeStateMachine.PerformTransition("Finished")
                # if big error need to finish with error (didn't reach goal)
            else:
                if not self._started_to_walk:
                    self._started_to_walk = True
                    ## Step in place: two first steps
                    self._foot_placement_path = self._GetTwoFirstStepFP_Path(
                        start_position, start_orientation, False)
                listSteps = []
                for desired in self._foot_placement_path:
                    command = AtlasSimInterfaceCommand()
                    step = command.step_params.desired_step
                    step.foot_index = desired.foot_index
                    step.swing_height = desired.clearance_height
                    step.pose.position.x = desired.pose.position.x
                    step.pose.position.y = desired.pose.position.y
                    step.pose.position.z = desired.pose.position.z
                    Q = quaternion_from_euler(desired.pose.ang_euler.x,
                                              desired.pose.ang_euler.y,
                                              desired.pose.ang_euler.z)
                    step.pose.orientation.x = Q[0]
                    step.pose.orientation.y = Q[1]
                    step.pose.orientation.z = Q[2]
                    step.pose.orientation.w = Q[3]
                    #print ("step command:",step)
                    listSteps.append(step)
                self._LPP.SetPath(listSteps)
                #print(listSteps)
        except rospy.ServiceException, e:
            print "Foot Placement Service call failed: %s" % e
Esempio n. 3
0
class QS_WalkingMode(WalkingMode):
    def __init__(self, iTf):
        self._LPP = QS_PathPlanner()
        WalkingMode.__init__(self, self._LPP)
        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)

        self._Odometer = Odometer()
        self._bDone = False
        self._bIsSwaying = False
        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)

    def Initialize(self, parameters):
        WalkingMode.Initialize(self, parameters)
        self._command = 0
        self._bRobotIsStatic = True

        rospy.wait_for_service('foot_placement_path')
        self._foot_placement_client = rospy.ServiceProxy(
            'foot_placement_path', FootPlacement_Service)
        # Subscribers:
        self._Subscribers["Odometry"] = rospy.Subscriber(
            '/C25/publish', C25C0_ROP, self._odom_cb)
        self._Subscribers["ASI_State"] = rospy.Subscriber(
            '/atlas/atlas_sim_interface_state', AtlasSimInterfaceState,
            self.asi_state_cb)
        self._Subscribers["IMU"] = rospy.Subscriber('/atlas/imu', Imu,
                                                    self._get_imu)
        self._Subscribers["JointStates"] = rospy.Subscriber(
            '/atlas/joint_states', JointState, self._get_joints)
        rospy.sleep(0.3)

        k_effort = [0] * 28
        k_effort[0:4] = 4 * [255]
        k_effort[16:28] = 12 * [255]
        self._JC.set_k_eff(k_effort)
        self._JC.set_all_pos(self._cur_jnt)
        self._JC.send_command()
        self._bDone = False
        self._bIsSwaying = False
        self._bRobotIsStatic = False
        self._RequestFootPlacements()
        self._GetOrientationDelta0Values(
        )  # Orientation difference between BDI odom and Global

        # Put robot into stand position
        stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
                                         None, None, None, None,
                                         self._k_effort)
        self.asi_command.publish(stand)
        rospy.sleep(0.3)

    def StartWalking(self):
        self._bDone = False

    def Walk(self):
        WalkingMode.Walk(self)
        self._command = self.GetCommand(self._BDI_state)
        self.asi_command.publish(self._command)
        #print(command)
        self._WalkingModeStateMachine.PerformTransition("Go")
        self._bIsSwaying = True

    def EmergencyStop(self):
        WalkingMode.Stop(self)

    def Stop(self):
        WalkingMode.Stop(self)

    def IsDone(self):
        return self._bDone

    def GetCommand(self, state):
        command = AtlasSimInterfaceCommand()
        command.behavior = AtlasSimInterfaceCommand.STEP
        #give user control over neck, back_z and arms
        command.k_effort = [0] * 28
        command.k_effort[2:4] = 2 * [255]
        command.k_effort[16:28] = 12 * [255]
        command.step_params.desired_step = self._LPP.GetNextStep()
        if (0 != command.step_params.desired_step):
            # Not sure why such a magic number
            command.step_params.desired_step.duration = 0.63
            # Apparently this next line is a must
            command.step_params.desired_step.step_index = 1
            command.step_params.desired_step = self._TransforFromGlobalToBDI(
                command.step_params.desired_step, state)
        else:
            command = 0
        return command

    def HandleStateMsg(self, state):
        command = 0
        if ("Idle" == self._WalkingModeStateMachine.GetCurrentState().Name):
            self._Odometer.SetPosition(state.pos_est.position.x,
                                       state.pos_est.position.y)
        elif ("Wait" == self._WalkingModeStateMachine.GetCurrentState().Name):
            self._Odometer.SetPosition(state.pos_est.position.x,
                                       state.pos_est.position.y)
            print("Odometer Updated")
            #print(2)
            self._WalkingModeStateMachine.PerformTransition("Go")
        elif ("Walking" == self._WalkingModeStateMachine.GetCurrentState().Name
              ):
            #print(3)
            if (QS_PathPlannerEnum.Active == self._LPP.State):
                command = self.GetCommand(state)
            elif (QS_PathPlannerEnum.Waiting == self._LPP.State):
                self._RequestFootPlacements()
        elif ("Done" == self._WalkingModeStateMachine.GetCurrentState().Name):
            #print(4)
            self._bDone = True
        else:
            raise Exception("QS_WalkingModeStateMachine::Bad State Name")

        return command

    def _RequestFootPlacements(self):
        print("Request Foot Placements")
        # Perform a service request from FP
        try:
            # Handle preemption?
            # if received a "End of mission" sort of message from FP
            start_pose, other_foot_pose = self._GetStartingFootPose()
            resp = self._foot_placement_client(0, start_pose, other_foot_pose)
            if [] == resp.foot_placement_path:  # 1 == resp.done:
                self._WalkingModeStateMachine.PerformTransition(
                    "Finished"
                )  # if array is empty need to finish with error (didn't reach goal)
            else:
                listSteps = []
                for desired in resp.foot_placement_path:
                    command = AtlasSimInterfaceCommand()
                    step = command.step_params.desired_step
                    step.foot_index = desired.foot_index
                    step.swing_height = desired.clearance_height
                    step.pose.position.x = desired.pose.position.x
                    step.pose.position.y = desired.pose.position.y
                    step.pose.position.z = desired.pose.position.z
                    Q = quaternion_from_euler(desired.pose.ang_euler.x,
                                              desired.pose.ang_euler.y,
                                              desired.pose.ang_euler.z)
                    step.pose.orientation.x = Q[0]
                    step.pose.orientation.y = Q[1]
                    step.pose.orientation.z = Q[2]
                    step.pose.orientation.w = Q[3]
                    listSteps.append(step)
                self._LPP.SetPath(listSteps)
                #print(listSteps)
        except rospy.ServiceException, e:
            print "Foot Placement Service call failed: %s" % e
Esempio n. 4
0
class WalkingModeBDI(WalkingMode):
    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 Initialize(self, parameters):
        WalkingMode.Initialize(self, parameters)
        self._Preview_Distance = 0.0  # planned ahead distance in BDI command
        # Subscriber
        self._Subscribers["Path"] = rospy.Subscriber('/path', C31_Waypoints,
                                                     self._path_cb)
        self._Subscribers["Odometry"] = rospy.Subscriber(
            '/C25/publish', C25C0_ROP, self._odom_cb)  #
        # self._Subscribers["Odometry"] = rospy.Subscriber('/ground_truth_odom',Odometry,self._odom_cb)
        self._Subscribers["ASI_State"] = rospy.Subscriber(
            '/atlas/atlas_sim_interface_state', AtlasSimInterfaceState,
            self.asi_state_cb)
        self._Subscribers["IMU"] = rospy.Subscriber('/atlas/imu', Imu,
                                                    self._get_imu)
        self._Subscribers["JointStates"] = rospy.Subscriber(
            '/atlas/joint_states', JointState, self._get_joints)
        rospy.sleep(0.3)

        self._bDone = False
        # # Puts robot into freeze behavior, all joints controlled
        # # Put the robot into a known state
        k_effort = [0] * 28
        k_effort[0:4] = 4 * [255]
        k_effort[16:28] = 12 * [255]
        self._JC.set_k_eff(k_effort)
        self._JC.set_all_pos(self._cur_jnt)
        self._JC.send_command()
        # freeze = AtlasSimInterfaceCommand(None,AtlasSimInterfaceCommand.FREEZE, None, None, None, None, k_effort )
        # self.asi_command.publish(freeze)

        # # Puts robot into stand_prep behavior, a joint configuration suitable
        # # to go into stand mode
        # stand_prep = AtlasSimInterfaceCommand(None,AtlasSimInterfaceCommand.STAND_PREP, None, None, None, None, k_effort)
        # self.asi_command.publish(stand_prep)

        # rospy.sleep(2.0)
        # self.mode.publish("nominal")

        # Put robot into stand position
        stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
                                         None, None, None, None, k_effort)

        rospy.sleep(0.3)

        self.asi_command.publish(stand)

        # Initialize some variables before starting.

        self.is_swaying = False
        self._BDI_StateMachine.Initialize(self.step_index_for_reset)

    def StartWalking(self):
        self._bDone = False
        return 0.3

    def Walk(self):
        WalkingMode.Walk(self)
        self._yaw = 0
        self._Odometer.SetYaw(self._yaw)

    def Stop(self):
        if (WalkingMode.Stop(self)):
            self._BDI_StateMachine.Stop()

    def EmergencyStop(self):
        # Puts robot into freeze behavior, all joints controlled
        # Put the robot into a known state
        k_effort = [0] * 28
        k_effort[3] = 255
        # freeze = AtlasSimInterfaceCommand(None,AtlasSimInterfaceCommand.FREEZE, None, None, None, None, k_effort )
        # self.asi_command.publish(freeze)
        # rospy.sleep(0.3)

        # Put robot into stand position
        stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
                                         None, None, None, None, k_effort)

    def IsDone(self):
        return self._bDone

###################################################################################
#--------------------------- CallBacks --------------------------------------------
###################################################################################

    def _path_cb(self, path):
        rospy.loginfo('got path %s', path)
        p = []
        for wp in path.points:
            p.append(Waypoint(wp.x, wp.y))
        self.SetPath(p)

    def _get_joints(self, msg):
        self._cur_jnt = msg.position

    # /atlas/atlas_sim_interface_state callback. Before publishing a walk command, we need
    # the current robot position
    def asi_state_cb(self, state):
        # if self._LPP.GetDoingQual() and (16.2 < self._LPP.GetPos().GetX()) and (self._LPP.GetPos().GetX() < 19.7) and not self._passedSteppingStones:
        #     if (True == self._setStep): # Initialize before stepping stones
        #         self.step_index = state.walk_feedback.next_step_index_needed -1
        #         step1,step2,step3,step4 = self.initSteppingStoneStepData(self.step_index, state)
        #         self._StepQueue.Initialize(step1,step2,step3,step4)
        #         self.GetSteppingStoneStepPlan(state)
        #         print("asi_state_cb:: SteppingStonesQueue length: ",self._SteppingStonesQueue.Length() )
        #         self._setStep = False
        #     command = self.SteppingStoneCommand(state);
        #     #print(command)
        # else:
        #     if self._LPP.GetDoingQual() and (False == self._setStep): # Initialize after stepping stones
        #         self._passedSteppingStones = True
        #         self._Odometer.SetPosition(state.pos_est.position.x,state.pos_est.position.y)
        #         print("asi_state_cb:: Initialize after stepping stones. Odometer X,Y: ",self._Odometer.GetGlobalPosition() )
        #         #self.Initialize()
        #         print("asi_state_cb:: Initialize after stepping stones. step index: ",self.step_index )
        #         WalkingMode.Initialize(self)
        #         self._BDI_StateMachine.Initialize(self.step_index)
        #         # self.step_index = state.behavior_feedback.walk_feedback.next_step_index_needed -1
        #         # step1,step2,step3,step4 = self.initSteppingStoneStepData(self.step_index, state)

        #     command = self.HandleStateMsg(state)
        #     self._bDone = self._WalkingModeStateMachine.IsDone()
        #     ######################
        #     self._setStep = True

        command = self._StateMachine.HandleStateMsg(state)
        self._bDone = self._StateMachine.IsDone()
        if (0 != command):
            self.asi_command.publish(command)

    def _odom_cb(self, odom):
        # SHOULD USE:
        self._LPP.UpdatePosition(
            odom.pose.pose.pose.position.x, odom.pose.pose.pose.position.y,
            self._Preview_Distance)  # from C25_GlobalPosition
        #self._LPP.UpdatePosition(odom.pose.pose.position.x,odom.pose.pose.position.y) # from /ground_truth_odom
        #self._odom_position = odom.pose.pose

    def _get_imu(self, msg):  #listen to /atlas/imu/pose/pose/orientation
        roll, pitch, self._yaw = euler_from_quaternion([
            msg.orientation.x, msg.orientation.y, msg.orientation.z,
            msg.orientation.w
        ])


###############################################################################################

    def SteppingStoneCommand(self, state):
        command = 0
        nextindex = state.walk_feedback.next_step_index_needed
        if (nextindex > self.step_index):
            self.step_index += 1
            command = self.GetCommand()
            # Instead of generating new steps, just pop a predefined queue
            stepData = self._StepQueue.Push(self._SteppingStonesQueue.Pop())
            #print("SteppingStoneCommand:: SteppingStonesQueue length: ",self._SteppingStonesQueue.Length() )
            #print("SteppingStoneCommand:: index: ",self.step_index)
        return command

    # def SteppingStoneCommandStatic(self, state):

    #     # When the robot status_flags are 1 (SWAYING), you can publish the next step command.
    #     if (state.behavior_feedback.step_feedback.status_flags == 1 and self._setStep):
    #         self.step_index += 1
    #         self._setStep = False
    #         print("Step " + str(self.step_index))
    #     elif (state.behavior_feedback.step_feedback.status_flags == 2):
    #         self._setStep = True
    #         print("No Step")

    #     is_right_foot = self.step_index % 2

    #     command = AtlasSimInterfaceCommand()
    #     command.behavior = AtlasSimInterfaceCommand.STEP

    #     # k_effort is all 0s for full bdi control of all joints
    #     command.k_effort = [0] * 28

    #     # step_index should always be one for a step command
    #     command.step_params.desired_step.step_index = 1
    #     command.step_params.desired_step.foot_index = is_right_foot

    #     # duration has far as I can tell is not observed
    #     command.step_params.desired_step.duration = 0.63

    #     # swing_height is not observed
    #     command.step_params.desired_step.swing_height = 0.1

    #     #if self.step_index > 30:
    #         #print(str(self.calculate_pose(self.step_index)))
    #     # Determine pose of the next step based on the number of steps we have taken
    #     command.step_params.desired_step.pose = self.calculate_pose(self.step_index,state)

    #     return command

    # # This method is used to calculate a pose of step based on the step_index
    # # The step poses just cause the robot to walk in a circle
    # def calculate_pose(self, step_index,state):
    #     # Right foot occurs on even steps, left on odd
    #     is_right_foot = step_index % 2
    #     is_left_foot = 1 - is_right_foot

    #     # Calculate orientation quaternion
    #     Q = quaternion_from_euler(0, 0, 0)
    #     pose = Pose()
    #     pose.position.x = state.pos_est.position.x - 3
    #     pose.position.y = state.pos_est.position.y + 0.15*is_left_foot

    #     # The z position is observed for static walking, but the foot
    #     # will be placed onto the ground if the ground is lower than z
    #     pose.position.z = 0

    #     pose.orientation.x = Q[0]
    #     pose.orientation.y = Q[1]
    #     pose.orientation.z = Q[2]
    #     pose.orientation.w = Q[3]

    #     return pose

    def GetCommand(self):
        command = AtlasSimInterfaceCommand()
        command.behavior = AtlasSimInterfaceCommand.WALK
        for i in range(4):
            command.walk_params.step_queue[i] = self._StepQueue.Peek(i)
        return command

    def initSteppingStoneStepData(self, step_index, state):
        print("initSteppingStoneStepData - index need to match:: cmd index = ",
              step_index, "state first index = ",
              state.walk_feedback.step_queue_saturated[0].step_index)
        stepData1 = copy.deepcopy(state.walk_feedback.step_queue_saturated[0])
        stepData2 = copy.deepcopy(state.walk_feedback.step_queue_saturated[1])
        stepData3 = copy.deepcopy(stepData2)
        stepData4 = copy.deepcopy(stepData2)

        # corrections to step 3 and 4: y position foot width corection +  ?alinement with stones?
        step_width = 0.25  # [meters]
        stepData3.step_index = step_index + 2
        stepData3.foot_index = stepData3.step_index % 2
        stepData4.step_index = step_index + 3
        if 0 == stepData3.foot_index:  # Left foot
            stepData3.pose.position.y = stepData2.pose.position.y + step_width
        else:  # Right foot
            stepData3.pose.position.y = stepData2.pose.position.y - step_width
        # stepData3.pose.orientation.x = 0.0
        # stepData3.pose.orientation.y = 0.0
        # stepData3.pose.orientation.z = 0.0
        # stepData3.pose.orientation.w = 1.0

        return stepData1, stepData2, stepData3, stepData4

    def GetSteppingStoneStepPlan(self, state):
        # foot placement path:
        path_start_foot_index = 1  # start stepping of SteppingStonesPath with Right foot
        #Stepping stones centers in world cord. [FootPlacement(16.7,8.0,0.0),FootPlacement(17.4,8.0,0.0),FootPlacement(17.9,8.5,0.0),FootPlacement(18.6,8.5,0.0),FootPlacement(19.1,8.0,0.0),FootPlacement(20.0,8.0,0.0)]
        self._SteppingStonesPath = [FootPlacement(16.9,7.87,0.0),FootPlacement(16.9,8.13,0.0),FootPlacement(17.3,7.87,0.0),FootPlacement(17.3,8.13,0.0),\
            FootPlacement(17.5,7.87,deg2r(30.0)),FootPlacement(17.5,8.2,deg2r(45.0)),FootPlacement(17.6,8.05,deg2r(45.0)),\
            FootPlacement(17.8,8.55,0.0),FootPlacement(17.8,8.35,0.0),FootPlacement(17.85,8.65,0.0),FootPlacement(18.0,8.35,0.0),FootPlacement(18.1,8.6,0.0),\
            FootPlacement(18.5,8.35,0.0),FootPlacement(18.5,8.6,0.0),FootPlacement(18.7,8.35,deg2r(-30.0)),FootPlacement(18.7,8.5,deg2r(-30.0)),\
            FootPlacement(19.0,8.0,deg2r(-30.0)),FootPlacement(19.0,8.17,0.0),FootPlacement(19.25,7.87,0.0),FootPlacement(19.25,8.13,0.0),\
            FootPlacement(19.65,7.87,0.0),FootPlacement(19.65,8.13,0.0),FootPlacement(19.9,7.87,0.0),FootPlacement(19.9,8.13,0.0),FootPlacement(20.0,7.87,0.0),FootPlacement(20.0,8.13,0.0),\
            FootPlacement(20.0,7.87,0.0),FootPlacement(20.0,8.13,0.0),FootPlacement(20.0,7.87,0.0),FootPlacement(20.0,8.13,0.0),FootPlacement(20.0,7.87,0.0),FootPlacement(20.0,8.13,0.0),\
            FootPlacement(20.0,7.87,0.0),FootPlacement(20.0,8.13,0.0),FootPlacement(20.0,7.87,0.0),FootPlacement(20.0,8.13,0.0),FootPlacement(20.0,7.87,0.0),FootPlacement(20.0,8.13,0.0)]
        ## building SteppingStonesQueue:
        # difference between BDI est position and LPP position (world cord.)
        deltaX = state.pos_est.position.x - self._LPP.GetPos().GetX()
        deltaY = state.pos_est.position.y - self._LPP.GetPos().GetY()
        deltaYaw = 0.0
        deltaFootPlacement = FootPlacement(deltaX, deltaY, deltaYaw)
        # feet positions for step data = self._SteppingStonesPath + deltaFootPlacement

        index = self._StepQueue.Peek(3).step_index  # + 1
        # step in place:
        # stepData1 = copy.deepcopy(self._StepQueue.Peek(2)) # using stepData3 to have the correct foot y position
        # stepData1.step_index = index
        # stepData1.foot_index = stepData1.step_index%2
        # index += 1
        # stepData2 = copy.deepcopy(self._StepQueue.Peek(3)) # using stepData3 to have the correct foot y position
        # stepData2.step_index = index
        # stepData2.foot_index = stepData2.step_index%2
        # self._SteppingStonesQueue.Append([stepData1, stepData2])
        if index % 2 == path_start_foot_index:
            index += 1
            stepData3 = copy.deepcopy(self._StepQueue.Peek(
                2))  # using stepData3 to have the correct foot y position
            stepData3.step_index = index
            stepData3.foot_index = stepData3.step_index % 2
            self._SteppingStonesQueue.Append([stepData3])

        index += 1
        for i in range(len(self._SteppingStonesPath)):
            # need to calc. step_index and foot_index for step data
            stepData = self._stepDataInit.GetStepData(
                index + i)  #self._StrategyForward
            stepData.pose.position.x = self._SteppingStonesPath[i].GetX(
            ) + deltaX
            stepData.pose.position.y = self._SteppingStonesPath[i].GetY(
            ) + deltaY
            step_yaw = self._SteppingStonesPath[i].GetYaw() + deltaYaw
            # Calculate orientation quaternion
            Q = quaternion_from_euler(0, 0, step_yaw)
            stepData.pose.orientation.x = Q[0]
            stepData.pose.orientation.y = Q[1]
            stepData.pose.orientation.z = Q[2]
            stepData.pose.orientation.w = Q[3]
            #print("GetSteppingStoneStepPlan:: step Data: ",stepData)
            self._SteppingStonesQueue.Append([stepData])

    def HandleStateMsg(self, state):
        step_queue_i = 3  # 0-3
        self._Preview_Distance = 0.3 + ( (state.pos_est.position.x-state.walk_feedback.step_queue_saturated[step_queue_i].pose.position.x)**2 + \
                                   (state.pos_est.position.y-state.walk_feedback.step_queue_saturated[step_queue_i].pose.position.y)**2  )**0.5
        if 2.0 <= self._Preview_Distance:
            self._Preview_Distance = 2.0
        command = 0
        if ("Idle" == self._WalkingModeStateMachine.GetCurrentState().Name):
            pass
        elif ("Wait" == self._WalkingModeStateMachine.GetCurrentState().Name):
            self.step_index_for_reset = state.walk_feedback.next_step_index_needed - 1
            self._Odometer.SetPosition(state.pos_est.position.x,
                                       state.pos_est.position.y)
            BDI_Static_orientation_q = state.foot_pos_est[0].orientation
            BDI_euler = euler_from_quaternion([
                BDI_Static_orientation_q.x, BDI_Static_orientation_q.y,
                BDI_Static_orientation_q.z, BDI_Static_orientation_q.w
            ])
            self._Odometer.SetYaw(BDI_euler[2])
            self._BDI_StateMachine.Initialize(self.step_index_for_reset)
            self._BDI_StateMachine.GoForward()
            self._WalkingModeStateMachine.PerformTransition("Go")
            #yaw?
        elif ("Walking" == self._WalkingModeStateMachine.GetCurrentState().Name
              ):
            if (self._LPP.IsActive()):
                # x,y = self._Odometer.GetGlobalPosition()
                # self._LPP.UpdatePosition(x,y)
                self._BDI_StateMachine.SetPathError(self._LPP.GetPathError())

                targetYaw = self._LPP.GetTargetYaw()
                delatYaw = targetYaw - self._Odometer.GetYaw()

                debug_transition_cmd = "NoCommand"
                if (math.sin(delatYaw) > 0.15):
                    #print("Sin(Delta)",math.sin(delatYaw), "Left")
                    debug_transition_cmd = "TurnLeft"
                    self._BDI_StateMachine.TurnLeft(targetYaw)
                elif (math.sin(delatYaw) < -0.15):
                    #print("Sin(Delta)",math.sin(delatYaw), "Right")
                    debug_transition_cmd = "TurnRight"
                    self._BDI_StateMachine.TurnRight(targetYaw)
            else:
                debug_transition_cmd = "Stop"
                self._BDI_StateMachine.Stop()
                if (self._BDI_StateMachine.IsDone()):
                    self._WalkingModeStateMachine.PerformTransition("Finished")

            command = self._BDI_StateMachine.Step(
                state.walk_feedback.next_step_index_needed)

            if (0 != command):
                rospy.loginfo(
                    "WalkingModeBDI, asi_state_cb: State Machine Transition Cmd = %s, Preview Distance=%f"
                    % (debug_transition_cmd, self._Preview_Distance))
        elif ("Done" == self._WalkingModeStateMachine.GetCurrentState().Name):
            pass
        else:
            raise Exception("BDI_WalkingModeStateMachine::Bad State Name")

        return command