Ejemplo n.º 1
0
    def dynamic(self, state):
        command = AtlasSimInterfaceCommand()
        command.behavior = AtlasSimInterfaceCommand.WALK

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

        # Observe next_step_index_needed to determine when to switch steps.
        self.step_index = state.walk_feedback.next_step_index_needed

        # A walk behavior command needs to know three additional steps beyond the current step needed to plan
        # for the best balance
        for i in range(4):
            step_index = self.step_index + i
            is_right_foot = step_index % 2

            command.walk_params.step_queue[i].step_index = step_index
            command.walk_params.step_queue[i].foot_index = is_right_foot

            # A duration of 0.63s is a good default value
            command.walk_params.step_queue[i].duration = 0.63

            # As far as I can tell, swing_height has yet to be implemented
            command.walk_params.step_queue[i].swing_height = 0.2

            # Determine pose of the next step based on the step_index
            command.walk_params.step_queue[i].pose = self.calculate_pose(
                step_index)

        # Publish this command every time we have a new state message
        self.asi_command.publish(command)
Ejemplo n.º 2
0
    def static(self, state):
        
        # When the robot status_flags are 1 (SWAYING), you can publish the next step command.
        if (state.step_feedback.status_flags == 1 and not self.is_swaying):
            self.step_index += 1
            self.is_swaying = True
            print("Step " + str(self.step_index))
        elif (state.step_feedback.status_flags == 2):
            self.is_swaying = False
        
        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 observed
        command.step_params.desired_step.swing_height = 0.4

        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)
        
        # Publish a new step command every time a state message is received
        self.asi_command.publish(command)
Ejemplo n.º 3
0
    def process_atlas(self, state):
        self.state = state;

        if not self._isWalking:
            return

        walk_goal = AtlasSimInterfaceCommand()
        walk_goal.behavior = AtlasSimInterfaceCommand.WALK
        walk_goal.walk_params.use_demo_walk = False

        # 0 for full BDI control, 255 for PID control
        walk_goal.k_effort =  [0] * 28
               
        # Load in the step array and pad it out
        steps = self.steps + [ self.steps[-1] ] * 5

        # Check if we have finished walking
        if state.walk_feedback.current_step_index >= len(self.steps) - 2:

            # Reset robot to standing
            stand_goal = AtlasSimInterfaceCommand()
            stand_goal.behavior = AtlasSimInterfaceCommand.STAND
            self.command.publish(stand_goal)

            # Clear step queue
            self.steps = []
            self._isWalking = False
            rospy.loginfo('Completed walk!')

        else:
            # Send next steps to ATLAS
            next_idx = state.walk_feedback.next_step_index_needed
            walk_goal.walk_params.step_queue = steps[next_idx : next_idx + 4]
            self.command.publish(walk_goal)
Ejemplo n.º 4
0
    def Step(self):
        if (3 < len(self._StepQueue)):
            command = AtlasSimInterfaceCommand()
            command.behavior = AtlasSimInterfaceCommand.WALK
            for i in range(4):
                command.walk_params.step_queue[i] = self._StepQueue[i]
        else:
            command = 0
#            print("Sending STAND Command")
#            command = AtlasSimInterfaceCommand(None,AtlasSimInterfaceCommand.STAND, None, None, None, None, [0]*28)
        self._StepQueue.popleft()
        self._index += 1
        return command
Ejemplo n.º 5
0
 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
Ejemplo n.º 6
0
 def EmergencyStop(self):
     #k_effort = [0] * 28
     #k_effort[3] = 255
     stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
                                      None, None, None, None,
                                      self._k_effort)
     self.asi_command.publish(stand)
Ejemplo n.º 7
0
 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
Ejemplo n.º 8
0
def harness():
    # Initialize atlas mode and atlas_sim_interface_command publishers
    _mode = rospy.Publisher('/atlas/mode', String, None, False, \
      True, None)
    _asi_command = rospy.Publisher('/atlas/atlas_sim_interface_command',
                                   AtlasSimInterfaceCommand, None, False, True,
                                   None)

    rospy.sleep(2.0)

    k_effort = [0] * 28
    # Puts robot into freeze behavior, all joints controlled
    freeze = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.FREEZE,
                                      None, None, None, None, k_effort)
    _asi_command.publish(freeze)

    rospy.sleep(1.0)
    # 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)
    _asi_command.publish(stand_prep)

    rospy.sleep(2.0)
    _mode.publish("nominal")

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

    rospy.sleep(0.3)

    _asi_command.publish(stand)

    rospy.wait_for_service('gazebo/reset_models')
    reset_gazebo_model = rospy.ServiceProxy('gazebo/reset_models', Empty)
    reset_gazebo_model()

    # Harness robot, with gravity off
    _mode.publish("harnessed")
    k_effort = [255] * 28
    # Puts robot into freeze behavior, all joints controlled
    freeze = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.USER,
                                      None, None, None, None, k_effort)
    _asi_command.publish(freeze)
Ejemplo n.º 9
0
 def send_stand_command(self):
     rospy.loginfo("set stand mode")
     _sim_msg = AtlasSimInterfaceCommand(behavior=0,
                                         k_effort=[
                                             0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                             0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                             0, 0, 0, 0, 0, 0, 0, 0
                                         ])
     _sim_msg.header.stamp = rospy.Time.now()
     self.sim_command_pub.publish(_sim_msg)
Ejemplo n.º 10
0
def move(x, y, z, w=0):
    # Initialize the node
    rospy.init_node('move_atlas')

    # Setup the publishers
    mode = rospy.Publisher('/atlas/mode', String, None, False, True, None)
    sim_interface = rospy.Publisher('/atlas/atlas_sim_interface_command',
                                    AtlasSimInterfaceCommand, None, False,
                                    True, None)
    set_pose = rospy.Publisher('/atlas/set_pose', Pose, None, False, True,
                               None)

    while set_pose.get_num_connections() == 0:
        rospy.sleep(0.1)
    while rospy.get_time() < 5.0:
        rospy.sleep(0.1)

    p = Pose()
    p.position.x = x
    p.position.y = y
    p.position.z = z
    p.orientation.x = 0
    p.orientation.y = 0
    p.orientation.z = sin(w / 2.0)
    p.orientation.w = cos(w / 2.0)

    com = AtlasSimInterfaceCommand()

    rospy.sleep(0.1)
    mode.publish("harnessed")
    com.behavior = com.FREEZE
    sim_interface.publish(com)
    com.behavior = com.STAND_PREP
    sim_interface.publish(com)
    rospy.sleep(2.0)
    mode.publish("nominal")
    rospy.sleep(0.05)
    set_pose.publish(p)
    rospy.sleep(0.2)
    com.behavior = com.STAND
    sim_interface.publish(com)
Ejemplo n.º 11
0
 def send_user_command(self):
     rospy.loginfo("set user mode")
     _sim_msg = AtlasSimInterfaceCommand(behavior=1,
                                         k_effort=[
                                             255, 255, 255, 255, 255, 255,
                                             255, 255, 255, 255, 255, 255,
                                             255, 255, 255, 255, 255, 255,
                                             255, 255, 255, 255, 255, 255,
                                             255, 255, 255, 255
                                         ])
     _sim_msg.header.stamp = rospy.Time.now()
     self.sim_command_pub.publish(_sim_msg)
Ejemplo n.º 12
0
 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
Ejemplo n.º 13
0
    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)
Ejemplo n.º 14
0
def move(x, y, z, w=0):
    # Initialize the node
    rospy.init_node('move_atlas')

    # Setup the publishers
    mode = rospy.Publisher('/atlas/mode', String, None, False, True, None)
    sim_interface = rospy.Publisher('/atlas/atlas_sim_interface_command', AtlasSimInterfaceCommand, None, False, True, None)
    set_pose= rospy.Publisher('/atlas/set_pose', Pose, None, False, True, None)

    while set_pose.get_num_connections() == 0:
          rospy.sleep(0.1)
    while rospy.get_time() < 5.0:
          rospy.sleep(0.1)

    p = Pose()
    p.position.x = x
    p.position.y = y
    p.position.z = z
    p.orientation.x = 0
    p.orientation.y = 0
    p.orientation.z = sin(w/2.0)
    p.orientation.w = cos(w/2.0)

    com = AtlasSimInterfaceCommand()

    rospy.sleep(0.1)
    mode.publish("harnessed")
    com.behavior = com.FREEZE
    sim_interface.publish(com)
    com.behavior = com.STAND_PREP
    sim_interface.publish(com)
    rospy.sleep(2.0)
    mode.publish("nominal")
    rospy.sleep(0.05)
    set_pose.publish(p)
    rospy.sleep(0.2)
    com.behavior = com.STAND
    sim_interface.publish(com)
Ejemplo n.º 15
0
    def _PrepareStepData(self):
        self._index += 1
        command = AtlasSimInterfaceCommand()

        stepData = command.walk_params.step_queue[0]
        stepData.step_index = self._index
        stepData.foot_index = self._index % 2

        stepData.duration = self._Duration
        stepData.swing_height = self._SwingHeight

        stepData.pose.position.z = 0.0

        return stepData
Ejemplo n.º 16
0
    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)
Ejemplo n.º 17
0
    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)
Ejemplo n.º 18
0
    def Initialize(self, parameters):
        WalkingMode.Initialize(self, parameters)
        self._CD_StateMachine.Initialize(self.step_index_for_reset)

        self._bDone = False
        self._yaw = 0

        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"
        # if resp_switched_to_BDI_odom:
        #     print "Using BDI odom"
        # else:
        #     print "Using ROBIL odom"

        # 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["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
        # 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()

        # 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)
Ejemplo n.º 19
0
    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)
Ejemplo n.º 20
0
    def demo(self):
        # Step 1: Go to stand-prep pose under user control
        stand_prep_msg = AtlasCommand()
        # Always insert current time
        stand_prep_msg.header.stamp = rospy.Time.now()
        # Assign some position and gain values that will get us there.
        stand_prep_msg.position = [
            2.438504816382192e-05, 0.0015186156379058957,
            9.983908967114985e-06, -0.0010675729718059301,
            -0.0003740221436601132, 0.06201673671603203, -0.2333149015903473,
            0.5181407332420349, -0.27610817551612854, -0.062101610004901886,
            0.00035181696875952184, -0.06218484416604042, -0.2332201600074768,
            0.51811283826828, -0.2762000858783722, 0.06211360543966293,
            0.29983898997306824, -1.303462266921997, 2.0007927417755127,
            0.49823325872421265, 0.0003098883025813848, -0.0044272784143686295,
            0.29982614517211914, 1.3034454584121704, 2.000779867172241,
            -0.498238742351532, 0.0003156556049361825, 0.004448802210390568
        ]
        stand_prep_msg.velocity = [0.0] * self.NUM_JOINTS
        stand_prep_msg.effort = [0.0] * self.NUM_JOINTS
        stand_prep_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Going to stand prep position...')
        self.ac_pub.publish(stand_prep_msg)
        time.sleep(2.0)

        # Step 2: Request BDI stand mode
        stand_msg = AtlasSimInterfaceCommand()
        # Always insert current time
        stand_msg.header.stamp = rospy.Time.now()
        # Tell it to stand
        stand_msg.behavior = stand_msg.STAND_PREP
        # Set k_effort = [255] to indicate that we still want all joints under user
        # control.  The stand behavior needs a few iterations before it start
        # outputting force values.
        stand_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Warming up BDI stand...')
        self.asic_pub.publish(stand_msg)
        time.sleep(1.0)
        # Now switch to stand
        stand_msg.behavior = stand_msg.STAND
        # Set k_effort = [0] to indicate that we want all joints under BDI control
        stand_msg.k_effort = [0] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[BDI] Standing...')
        self.asic_pub.publish(stand_msg)
        time.sleep(2.0)

        # Step 3: Move the arms and head a little (not too much; don't want to fall
        # over)
        slight_movement_msg = AtlasCommand()
        # Always insert current time
        slight_movement_msg.header.stamp = rospy.Time.now()
        # Start with 0.0 and set values for the joints that we want to control
        slight_movement_msg.position = [0.0] * self.NUM_JOINTS
        slight_movement_msg.position[AtlasState.neck_ry] = -0.1
        slight_movement_msg.position[AtlasState.l_arm_ely] = 2.1
        slight_movement_msg.position[AtlasState.l_arm_wrx] = -0.1
        slight_movement_msg.position[AtlasState.r_arm_ely] = 2.1
        slight_movement_msg.position[AtlasState.r_arm_wrx] = -0.1
        slight_movement_msg.velocity = [0.0] * self.NUM_JOINTS
        slight_movement_msg.effort = [0.0] * self.NUM_JOINTS
        slight_movement_msg.kp_position = [
            20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0,
            300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0,
            200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0,
            100.0
        ]
        slight_movement_msg.ki_position = [0.0] * self.NUM_JOINTS
        slight_movement_msg.kd_position = [0.0] * self.NUM_JOINTS
        # Bump up kp_velocity to reduce the jerkiness of the transition
        stand_prep_msg.kp_velocity = [50.0] * self.NUM_JOINTS
        slight_movement_msg.i_effort_min = [0.0] * self.NUM_JOINTS
        slight_movement_msg.i_effort_max = [0.0] * self.NUM_JOINTS
        # Set k_effort = [1] for the joints that we want to control.
        # BDI has control of the other joints
        slight_movement_msg.k_effort = [0] * self.NUM_JOINTS
        slight_movement_msg.k_effort[AtlasState.neck_ry] = 255
        slight_movement_msg.k_effort[AtlasState.l_arm_ely] = 255
        slight_movement_msg.k_effort[AtlasState.l_arm_wrx] = 255
        slight_movement_msg.k_effort[AtlasState.r_arm_ely] = 255
        slight_movement_msg.k_effort[AtlasState.r_arm_wrx] = 255
        # Publish and give time to take effect
        print('[USER/BDI] Command neck and arms...')
        self.ac_pub.publish(slight_movement_msg)
        time.sleep(2.0)

        # Step 4: Request BDI walk
        walk_msg = AtlasSimInterfaceCommand()
        # Always insert current time
        walk_msg.header.stamp = rospy.Time.now()
        # Tell it to walk
        walk_msg.behavior = walk_msg.WALK
        walk_msg.walk_params.use_demo_walk = False
        # Fill in some steps
        for i in range(4):
            step_data = AtlasBehaviorStepData()
            # Steps are indexed starting at 1
            step_data.step_index = i + 1
            # 0 = left, 1 = right
            step_data.foot_index = i % 2
            # 0.3 is a good number
            step_data.swing_height = 0.3
            # 0.63 is a good number
            step_data.duration = 0.63
            # We'll specify desired foot poses in ego-centric frame then
            # transform them into the robot's world frame.
            # Match feet so that we end with them together
            step_data.pose.position.x = (1 + i / 2) * 0.25
            # Step 0.15m to either side of center, alternating with feet
            step_data.pose.position.y = 0.15 if (i % 2 == 0) else -0.15
            step_data.pose.position.z = 0.0
            # Point those feet straight ahead
            step_data.pose.orientation.x = 0.0
            step_data.pose.orientation.y = 0.0
            step_data.pose.orientation.z = 0.0
            step_data.pose.orientation.w = 1.0
            # Transform this foot pose according to robot's
            # current estimated pose in the world, which is a combination of IMU
            # and internal position estimation.
            # http://www.ros.org/wiki/kdl/Tutorials/Frame%20transformations%20%28Python%29
            f1 = posemath.fromMsg(step_data.pose)
            f2 = PyKDL.Frame(
                PyKDL.Rotation.Quaternion(self.imu_msg.orientation.x,
                                          self.imu_msg.orientation.y,
                                          self.imu_msg.orientation.z,
                                          self.imu_msg.orientation.w),
                PyKDL.Vector(self.asis_msg.pos_est.position.x,
                             self.asis_msg.pos_est.position.y,
                             self.asis_msg.pos_est.position.z))
            f = f2 * f1
            step_data.pose = posemath.toMsg(f)
            walk_msg.walk_params.step_queue[i] = step_data
        # Use the same k_effort from the last step, to retain user control over some
        # joints. BDI has control of the other joints.
        walk_msg.k_effort = slight_movement_msg.k_effort
        # Publish and give time to take effect
        print('[USER/BDI] Walking...')
        self.asic_pub.publish(walk_msg)
        time.sleep(6.0)

        # Step 5: Go back to home pose under user control
        home_msg = AtlasCommand()
        # Always insert current time
        home_msg.header.stamp = rospy.Time.now()
        # Assign some position and gain values that will get us there.
        home_msg.position = [0.0] * self.NUM_JOINTS
        home_msg.velocity = [0.0] * self.NUM_JOINTS
        home_msg.effort = [0.0] * self.NUM_JOINTS
        home_msg.kp_position = [
            20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0,
            300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0,
            200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0,
            100.0
        ]
        home_msg.ki_position = [0.0] * self.NUM_JOINTS
        home_msg.kd_position = [0.0] * self.NUM_JOINTS
        # Bump up kp_velocity to reduce the jerkiness of the transition
        home_msg.kp_velocity = [50.0] * self.NUM_JOINTS
        home_msg.i_effort_min = [0.0] * self.NUM_JOINTS
        home_msg.i_effort_max = [0.0] * self.NUM_JOINTS
        # Set k_effort = [1] to indicate that we want all joints under user control
        home_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Going to home position...')
        self.ac_pub.publish(home_msg)
        time.sleep(2.0)
Ejemplo n.º 21
0
 def _switch_mode_to_manual(self):
     message = AtlasSimInterfaceCommand()
     message.header.stamp = rospy.Time.now()
     message.behavior = AtlasSimInterfaceCommand.USER
     self._atlas_sim_interface_command_publisher.publish(message)
     time.sleep(0.5)
Ejemplo n.º 22
0
 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
Ejemplo n.º 23
0
        'r_arm_ely',
        'r_arm_elx',
        'r_arm_uwy',
        'r_arm_mwx'
    ]  #27
    _JC = JointCommands_msg_handler(robot_name, jnt_names)
    _k_effort = [0] * 28
    #self._k_effort[3] = 255
    _k_effort[3:4] = 1 * [255]
    _k_effort[16:28] = 12 * [255]
    _JC.set_k_eff(_k_effort)
    _JC.set_all_pos(_cur_jnt)
    _JC.send_command()

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

#
#class CD_WalkingMode(WalkingMode):
#
#class CD_WalkingMode(WalkingMode):
#    def __init__(self):
#        WalkingMode.__init__(self,CD_PathPlanner())
#        self.step_index_for_reset = 0
#        # Initialize atlas mode and atlas_sim_interface_command publishers
#
#
#        self._CD_StateMachine = CD_StateMachine(self._LPP)
Ejemplo n.º 24
0
    def demo(self):
        # Step 0: Go to home pose under user control
        home_msg = AtlasCommand()
        # Always insert current time
        home_msg.header.stamp = rospy.Time.now()
        # Assign some position and gain values that will get us there.
        home_msg.position = [0.0] * self.NUM_JOINTS
        home_msg.velocity = [0.0] * self.NUM_JOINTS
        home_msg.effort = [0.0] * self.NUM_JOINTS
        home_msg.kp_position = [20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0]
        home_msg.ki_position = [0.0] * self.NUM_JOINTS
        home_msg.kd_position = [0.0] * self.NUM_JOINTS
        # Bump up kp_velocity to reduce the jerkiness of the transition
        home_msg.kp_velocity = [50.0] * self.NUM_JOINTS
        home_msg.i_effort_min = [0.0] * self.NUM_JOINTS
        home_msg.i_effort_max = [0.0] * self.NUM_JOINTS 
        # Set k_effort = [1] to indicate that we want all joints under user control
        home_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Going to home position...')
        self.ac_pub.publish(home_msg)
        time.sleep(2.0)
    
        # Step 1: Go to stand-prep pose under user control
        stand_prep_msg = AtlasCommand()
        # Always insert current time
        stand_prep_msg.header.stamp = rospy.Time.now()
        # Assign some position and gain values that will get us there.
        stand_prep_msg.position = [2.438504816382192e-05, 0.0015186156379058957, 9.983908967114985e-06, -0.0010675729718059301, -0.0003740221436601132, 0.06201673671603203, -0.2333149015903473, 0.5181407332420349, -0.27610817551612854, -0.062101610004901886, 0.00035181696875952184, -0.06218484416604042, -0.2332201600074768, 0.51811283826828, -0.2762000858783722, 0.06211360543966293, 0.29983898997306824, -1.303462266921997, 2.0007927417755127, 0.49823325872421265, 0.0003098883025813848, -0.0044272784143686295, 0.29982614517211914, 1.3034454584121704, 2.000779867172241, -0.498238742351532, 0.0003156556049361825, 0.004448802210390568]
        stand_prep_msg.velocity = [0.0] * self.NUM_JOINTS
        stand_prep_msg.effort = [0.0] * self.NUM_JOINTS
        stand_prep_msg.kp_position = [20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0]
        stand_prep_msg.ki_position = [0.0] * self.NUM_JOINTS
        stand_prep_msg.kd_position = [0.0] * self.NUM_JOINTS
        # Bump up kp_velocity to reduce the jerkiness of the transition
        stand_prep_msg.kp_velocity = [50.0] * self.NUM_JOINTS
        stand_prep_msg.i_effort_min = [0.0] * self.NUM_JOINTS
        stand_prep_msg.i_effort_max = [0.0] * self.NUM_JOINTS 
        # Set k_effort = [1] to indicate that we want all joints under user control
        stand_prep_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Going to stand prep position...')
        self.ac_pub.publish(stand_prep_msg)
        time.sleep(2.0)
    
        # Step 2: Request BDI stand mode
        stand_msg = AtlasSimInterfaceCommand()
        # Always insert current time
        stand_msg.header.stamp = rospy.Time.now()
        # Tell it to stand
        stand_msg.behavior = stand_msg.STAND_PREP
        # Set k_effort = [255] to indicate that we still want all joints under user
        # control.  The stand behavior needs a few iterations before it start
        # outputting force values.
        stand_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Warming up BDI stand...')
        self.asic_pub.publish(stand_msg)
        time.sleep(1.0)
        # Now switch to stand
        stand_msg.behavior = stand_msg.STAND
        # Set k_effort = [0] to indicate that we want all joints under BDI control
        stand_msg.k_effort = [0] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[BDI] Standing...')
        self.asic_pub.publish(stand_msg)
        time.sleep(2.0)
     
        # Step 3: Move the arms and head a little (not too much; don't want to fall
        # over)
        slight_movement_msg = AtlasCommand()
        # Always insert current time
        slight_movement_msg.header.stamp = rospy.Time.now()
        # Start with 0.0 and set values for the joints that we want to control
        slight_movement_msg.position = [0.0] * self.NUM_JOINTS
        slight_movement_msg.position[AtlasState.neck_ay] = -0.1
        slight_movement_msg.position[AtlasState.l_arm_ely] = 2.1
        slight_movement_msg.position[AtlasState.l_arm_mwx] = -0.1
        slight_movement_msg.position[AtlasState.r_arm_ely] = 2.1
        slight_movement_msg.position[AtlasState.r_arm_mwx] = -0.1
        slight_movement_msg.velocity = [0.0] * self.NUM_JOINTS
        slight_movement_msg.effort = [0.0] * self.NUM_JOINTS
        slight_movement_msg.kp_position = [20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0]
        slight_movement_msg.ki_position = [0.0] * self.NUM_JOINTS
        slight_movement_msg.kd_position = [0.0] * self.NUM_JOINTS
        # Bump up kp_velocity to reduce the jerkiness of the transition
        stand_prep_msg.kp_velocity = [50.0] * self.NUM_JOINTS
        slight_movement_msg.i_effort_min = [0.0] * self.NUM_JOINTS
        slight_movement_msg.i_effort_max = [0.0] * self.NUM_JOINTS 
        # Set k_effort = [1] for the joints that we want to control.
        # BDI has control of the other joints
        slight_movement_msg.k_effort = [0] * self.NUM_JOINTS
        slight_movement_msg.k_effort[AtlasState.neck_ay] = 255
        slight_movement_msg.k_effort[AtlasState.l_arm_ely] = 255
        slight_movement_msg.k_effort[AtlasState.l_arm_mwx] = 255
        slight_movement_msg.k_effort[AtlasState.r_arm_ely] = 255
        slight_movement_msg.k_effort[AtlasState.r_arm_mwx] = 255
        # Publish and give time to take effect
        print('[USER/BDI] Command neck and arms...')
        self.ac_pub.publish(slight_movement_msg)
        time.sleep(2.0)
    
        # Step 4: Request BDI walk
        walk_msg = AtlasSimInterfaceCommand()
        # Always insert current time
        walk_msg.header.stamp = rospy.Time.now()
        # Tell it to walk
        walk_msg.behavior = walk_msg.WALK
        walk_msg.walk_params.use_demo_walk = False
        # Fill in some steps
        for i in range(4):
            step_data = AtlasBehaviorStepData()
            # Steps are indexed starting at 1
            step_data.step_index = i+1
            # 0 = left, 1 = right
            step_data.foot_index = i%2
            # 0.3 is a good number
            step_data.swing_height = 0.3
            # 0.63 is a good number
            step_data.duration = 0.63
            # We'll specify desired foot poses in ego-centric frame then
            # transform them into the robot's world frame.
            # Match feet so that we end with them together
            step_data.pose.position.x = (1+i/2)*0.25
            # Step 0.15m to either side of center, alternating with feet
            step_data.pose.position.y = 0.15 if (i%2==0) else -0.15
            step_data.pose.position.z = 0.0
            # Point those feet straight ahead
            step_data.pose.orientation.x = 0.0
            step_data.pose.orientation.y = 0.0
            step_data.pose.orientation.z = 0.0
            step_data.pose.orientation.w = 1.0
            # Transform this foot pose according to robot's
            # current estimated pose in the world, which is a combination of IMU
            # and internal position estimation.
            # http://www.ros.org/wiki/kdl/Tutorials/Frame%20transformations%20%28Python%29
            f1 = posemath.fromMsg(step_data.pose)
            f2 = PyKDL.Frame(PyKDL.Rotation.Quaternion(self.imu_msg.orientation.x,
                                                       self.imu_msg.orientation.y,
                                                       self.imu_msg.orientation.z,
                                                       self.imu_msg.orientation.w),
                             PyKDL.Vector(self.asis_msg.pos_est.position.x,
                                          self.asis_msg.pos_est.position.y,
                                          self.asis_msg.pos_est.position.z))
            f = f2 * f1
            step_data.pose = posemath.toMsg(f)
            walk_msg.walk_params.step_queue[i] = step_data
        # Use the same k_effort from the last step, to retain user control over some
        # joints. BDI has control of the other joints.
        walk_msg.k_effort = slight_movement_msg.k_effort
        # Publish and give time to take effect
        print('[USER/BDI] Walking...')
        self.asic_pub.publish(walk_msg)
        time.sleep(6.0)

        # Step 5: Go back to home pose under user control
        home_msg = AtlasCommand()
        # Always insert current time
        home_msg.header.stamp = rospy.Time.now()
        # Assign some position and gain values that will get us there.
        home_msg.position = [0.0] * self.NUM_JOINTS
        home_msg.velocity = [0.0] * self.NUM_JOINTS
        home_msg.effort = [0.0] * self.NUM_JOINTS
        home_msg.kp_position = [20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0]
        home_msg.ki_position = [0.0] * self.NUM_JOINTS
        home_msg.kd_position = [0.0] * self.NUM_JOINTS
        # Bump up kp_velocity to reduce the jerkiness of the transition
        home_msg.kp_velocity = [50.0] * self.NUM_JOINTS
        home_msg.i_effort_min = [0.0] * self.NUM_JOINTS
        home_msg.i_effort_max = [0.0] * self.NUM_JOINTS 
        # Set k_effort = [1] to indicate that we want all joints under user control
        home_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Going to home position...')
        self.ac_pub.publish(home_msg)
        time.sleep(2.0)
Ejemplo n.º 25
0
    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
Ejemplo n.º 26
0
    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)