Esempio n. 1
0
    def add_step(self, radius):
        L = self.params["Forward Stride Length"]["value"]
        W = self.params["Stride Width"]["value"]
        T = self.params["In Place Turn Size"]["value"]
        R = radius

        # Get origin based on last step
        if not self.steps:
            raise Exception("Unknown foot positions.")

        last_step = self.steps[-1]
        rpy = euler_from_quaternion([
                last_step.pose.orientation.x,
                last_step.pose.orientation.y,
                last_step.pose.orientation.z,
                last_step.pose.orientation.w,
                ])
        
        Th = rpy[2]
        X = last_step.pose.position.x + signum(last_step.foot_index) * W/2 * math.sin(Th)
        Y = last_step.pose.position.y - signum(last_step.foot_index) * W/2 * math.cos(Th)
        
        # Transform from here to next step center
        if (R > L):
            dTh = math.asin(L / R)
        else:
            dTh = signum(R) * T

        dX = R * math.sin(dTh)
        dY = R * (1 - math.cos(dTh))
        
        # Transform to next foot position
        dX = dX + signum(last_step.foot_index) * W/2 * math.sin(dTh)
        dY = dY - signum(last_step.foot_index) * W/2 * math.cos(dTh)

        # Store this foot position
        Q = quaternion_from_euler(0, 0, Th + dTh)

        step = AtlasBehaviorStepData()
        step.step_index = last_step.step_index + 1
        step.foot_index = 1 if (last_step.foot_index == 0) else 0
        step.duration = self.params["Stride Duration"]["value"]
            
        step.pose.position.x = X + math.cos(Th)*dX - math.sin(Th)*dY
        step.pose.position.y = Y + math.sin(Th)*dX + math.cos(Th)*dY
        step.pose.position.z = self.params["Step Height"]["value"]
         
        step.pose.orientation.x = Q[0]
        step.pose.orientation.y = Q[1]
        step.pose.orientation.z = Q[2]
        step.pose.orientation.w = Q[3]
            
        step.swing_height = self.params["Swing Height"]["value"]         
        self.steps.append(step)
Esempio n. 2
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)
Esempio n. 3
0
    def twist(self, forward, lateral, turn):
        steps = []

        L = self.params["Forward Stride Length"]["value"]
        L_lat = self.params["Lateral Stride Length"]["value"]
        R = self.params["Turn Radius"]["value"]
        W = self.params["Stride Width"]["value"]
        X = 0
        Y = 0
        theta = 0
        dTheta = 0

        if forward != 0:
            dTheta = turn * 2 * math.asin(L / (2 * (R + self.params["Stride Width"]["value"] / 2)))
        else:
            dTheta = turn * self.params["In Place Turn Size"]["value"]
        steps = []

        # This home step doesn't currently do anything, but it's a
        # response to bdi not visiting the first step in a trajectory
        home_step = AtlasBehaviorStepData()

        # If moving right, first dummy step is on the left
        home_step.foot_index = 1 * (lateral < 0)
        home_step.pose.position.y = 0.1
        steps.append(home_step)
        prevX = 0
        prevY = 0

        # Builds the sequence of steps needed
        for i in range(self.params["Walk Sequence Length"]["value"]):
            theta += (turn != 0) * dTheta

            # is_right_foot = 1, when stepping with right
            is_even = i % 2
            is_odd = 1 - is_even
            is_right_foot = is_even
            is_left_foot = is_odd

            # left = 1, right = -1
            foot = 1 - 2 * is_right_foot

            if turn == 0:
                X = (forward != 0) * (X + forward * L)
                Y = (lateral != 0) * (Y + is_odd * lateral * L_lat) + foot * W / 2
            elif forward != 0:
                # Radius from point to foot (if turning)
                R_foot = R + foot * W / 2

                # turn > 0 for CCW, turn < 0 for CW
                X = forward * turn * R_foot * math.sin(theta)
                Y = forward * turn * (R - R_foot * math.cos(theta))

                self.debuginfo(
                    "R: "
                    + str(R)
                    + " R_foot:"
                    + str(R_foot)
                    + " theta: "
                    + str(theta)
                    + " math.sin(theta): "
                    + str(math.sin(theta))
                    + " math.cos(theta) + "
                    + str(math.cos(theta))
                )
            elif turn != 0:
                X = turn * W / 2 * math.sin(theta)
                Y = turn * W / 2 * math.cos(theta)

            Q = quaternion_from_euler(0, 0, theta)
            step = AtlasBehaviorStepData()

            # One step already exists, so add one to index
            step.step_index = i + 1

            # Alternate between feet, start with left
            step.foot_index = is_right_foot

            # If moving laterally to the left, start with the right foot
            if lateral > 0:
                step.foot_index = is_left_foot

            step.duration = self.params["Stride Duration"]["value"]

            step.pose.position.x = X
            step.pose.position.y = Y
            step.pose.position.z = self.params["Step Height"]["value"]

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

            step.swing_height = self.params["Swing Height"]["value"]
            steps.append(step)

        # Add final step to bring feet together
        is_right_foot = 1 - steps[-1].foot_index
        is_even = is_right_foot
        # foot = 1 for left, foot = -1 for right
        foot = 1 - 2 * is_right_foot

        if turn == 0:
            Y = Y - foot * W
        elif forward != 0:
            self.debuginfo(
                "R: "
                + str(R)
                + " R_foot:"
                + str(R_foot)
                + " theta: "
                + str(theta)
                + " math.sin(theta): "
                + str(math.sin(theta))
                + " math.cos(theta) + "
                + str(math.cos(theta))
            )

            # R_foot is radius to foot
            R_foot = R + foot * W / 2
            # turn > 0 for counter clockwise
            X = forward * turn * R_foot * math.sin(theta)
            Y = forward * turn * (R - R_foot * math.cos(theta))
        else:
            X = turn * W / 2 * math.sin(theta)
            Y = turn * W / 2 * math.cos(theta)

        Q = quaternion_from_euler(0, 0, theta)
        step = AtlasBehaviorStepData()
        step.step_index = len(steps)
        step.foot_index = is_right_foot
        step.duration = self.params["Stride Duration"]["value"]
        step.pose.position.x = X
        step.pose.position.y = Y
        step.pose.position.z = self.params["Step Height"]["value"]
        step.pose.orientation.x = Q[0]
        step.pose.orientation.y = Q[1]
        step.pose.orientation.z = Q[2]
        step.pose.orientation.w = Q[3]
        step.swing_height = self.params["Swing Height"]["value"]

        steps.append(step)

        # 0 for full BDI control, 255 for PID control
        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]

        walk_goal = WalkDemoGoal(
            Header(),
            WalkDemoGoal.WALK,
            steps,
            AtlasBehaviorStepParams(),
            AtlasBehaviorStandParams(),
            AtlasBehaviorManipulateParams(),
            k_effort,
        )

        self.client.send_goal(walk_goal)
        for step in steps:
            # self.loginfo("step: " + str(step))
            self.loginfo(
                "foot: "
                + str(step.foot_index)
                + " ["
                + str(step.pose.position.x)
                + ", "
                + str(step.pose.position.y)
                + ", "
                + str(theta)
                + "]"
            )
        self.loginfo("")

        self.client.wait_for_result(rospy.Duration(2 * self.params["Stride Duration"]["value"] * len(steps) + 5))
Esempio n. 4
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)
Esempio n. 5
0
    def build_steps(self, forward, lateral, turn):
        L = self.params["Forward Stride Length"]["value"]
        L_lat = self.params["Lateral Stride Length"]["value"]
        R = self.params["Turn Radius"]["value"]
        W = self.params["Stride Width"]["value"]
        X = 0
        Y = 0
        theta = 0
        dTheta = 0

        if forward != 0:
            dTheta = turn * 2 * math.asin(L / (2 * (R + \
            self.params["Stride Width"]["value"]/2)))
        else:
            dTheta = turn * self.params["In Place Turn Size"]["value"]
        steps = []


        if forward != 0:
            dTheta = turn * 2 * math.asin(L / (2 * (R + \
            self.params["Stride Width"]["value"]/2)))
        else:
            dTheta = turn * self.params["In Place Turn Size"]["value"]
        steps = []

        # This home step doesn't currently do anything, but it's a
        # response to bdi not visiting the first step in a trajectory
        # home_step = AtlasBehaviorStepData()

        # If moving right, first dummy step is on the left
        # home_step.foot_index = 1*(lateral < 0)
        # home_step.pose.position.y = 0.1
        # steps.append(home_step)
        prevX = 0
        prevY = 0

        # Builds the sequence of steps needed
        for i in range(self.params["Walk Sequence Length"]["value"]):
            # is_right_foot = 1, when stepping with right
            is_even = i%2
            is_odd = 1 - is_even
            is_right_foot = is_even
            is_left_foot = is_odd

            # left = 1, right = -1
            foot = 1 - 2 * is_right_foot

            if self.is_static:
                theta = (turn != 0) * dTheta
                if turn == 0:
                    X = (forward != 0) * (forward * L)
                    Y = (lateral != 0) * (is_odd * lateral * L_lat) + \
                        foot * W / 2
                elif forward != 0:
                    # Radius from point to foot (if turning)
                    R_foot = R + foot * W/2

                    # turn > 0 for CCW, turn < 0 for CW
                    X = forward * turn * R_foot * math.sin(theta)
                    Y = forward * turn * (R - R_foot*math.cos(theta))

                    self.debuginfo("R: " + str(R) + " R_foot:" + \
                    str(R_foot) + " theta: " + str(theta) +  \
                    " math.sin(theta): " + str(math.sin(theta)) + \
                    " math.cos(theta) + " + str(math.cos(theta)))
                elif turn != 0:
                    X = turn * W/2 * math.sin(theta)
                    Y = turn * W/2 * math.cos(theta)
            else:
                theta += (turn != 0) * dTheta
                if turn == 0:
                    X = (forward != 0) * (X + forward * L)
                    Y = (lateral != 0) * (Y + is_odd * lateral * L_lat) + \
                        foot * W / 2
                elif forward != 0:
                    # Radius from point to foot (if turning)
                    R_foot = R + foot * W/2

                    # turn > 0 for CCW, turn < 0 for CW
                    X = forward * turn * R_foot * math.sin(theta)
                    Y = forward * turn * (R - R_foot*math.cos(theta))

                    self.debuginfo("R: " + str(R) + " R_foot:" + \
                    str(R_foot) + " theta: " + str(theta) +  \
                    " math.sin(theta): " + str(math.sin(theta)) + \
                    " math.cos(theta) + " + str(math.cos(theta)))
                elif turn != 0:
                    X = turn * W/2 * math.sin(theta)
                    Y = turn * W/2 * math.cos(theta)


            Q = quaternion_from_euler(0, 0, theta)
            step = AtlasBehaviorStepData()

            # One step already exists, so add one to index
            step.step_index = i

            # Alternate between feet, start with left
            step.foot_index = is_right_foot

            #If moving laterally to the left, start with the right foot
            if (lateral > 0):
                step.foot_index = is_left_foot

            step.duration = self.params["Stride Duration"]["value"]

            step.pose.position.x = X
            step.pose.position.y = Y
            step.pose.position.z = self.params["Step Height"]["value"]

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

            step.swing_height = self.params["Swing Height"]["value"]
            steps.append(step)

        # Add final step to bring feet together
        is_right_foot = 1 - steps[-1].foot_index
        is_even = is_right_foot
        # foot = 1 for left, foot = -1 for right
        foot = 1 - 2 * is_right_foot

        if turn == 0:
            Y = Y + foot * W
        elif forward != 0:
            self.debuginfo("R: " + str(R) + " R_foot:" + \
            str(R_foot) + " theta: " + str(theta) +  \
           " math.sin(theta): " + str(math.sin(theta)) + \
           " math.cos(theta) + " + str(math.cos(theta)))

            # R_foot is radius to foot
            R_foot = R + foot * W/2
            #turn > 0 for counter clockwise
            X = forward * turn * R_foot * math.sin(theta)
            Y = forward * turn * (R - R_foot*math.cos(theta))
        else:
            X = turn * W/2 * math.sin(theta)
            Y = turn * W/2 * math.cos(theta)

        Q = quaternion_from_euler(0, 0, theta)
        step = AtlasBehaviorStepData()
        step.step_index = len(steps)
        step.foot_index = is_right_foot
        step.duration = self.params["Stride Duration"]["value"]
        step.pose.position.x = X
        step.pose.position.y = Y
        step.pose.position.z = self.params["Step Height"]["value"]
        step.pose.orientation.x = Q[0]
        step.pose.orientation.y = Q[1]
        step.pose.orientation.z = Q[2]
        step.pose.orientation.w = Q[3]
        step.swing_height = self.params["Swing Height"]["value"]

        steps.append(step)

        return steps
Esempio n. 6
0
    def twist(self, forward, lateral, turn):

        steps = []
        
        L = rospy.get_param("Forward_Stride_Length")
        L_lat = rospy.get_param("Lateral_Stride_Length")
        R = rospy.get_param("Turn_Radius")
        W = rospy.get_param("Stride_Width")
        X = 0
        Y = 0
        theta = 0
        dTheta = 0
        
        if forward != 0:
            dTheta = turn * 2 * math.asin(L / (2 * (R + rospy.get_param("Stride_Width")/2)))
        else:
            dTheta = turn * rospy.get_param("In_Place_Turn_Size")
        steps = []
        
        # This home step doesn't currently do anything, but it's a 
        # response to bdi not visiting the first step in a trajectory
        home_step = AtlasBehaviorStepData()
        
        # If moving right, first dummy step is on the left
        home_step.foot_index = 1*(lateral < 0)
        home_step.pose.position.y = 0.1
        steps.append(home_step)
        prevX = 0
        prevY = 0
        
        # Builds the sequence of steps needed
        for i in range(rospy.get_param("Walk_Sequence_Length")):
            theta += (turn != 0) * dTheta
            
            # is_right_foot = 1, when stepping with right
            is_even = i%2
            is_odd = 1 - is_even
            is_right_foot = is_even
            is_left_foot = is_odd

            # left = 1, right = -1            
            foot = 1 - 2 * is_right_foot
            
            if turn == 0:
                X = (forward != 0) * (X + forward * L)
                Y = (lateral != 0) * (Y + is_odd * lateral * L_lat) + foot * W / 2
            elif forward != 0:
                # Radius from point to foot (if turning)
                R_foot = R + foot * W/2
                
                # turn > 0 for CCW, turn < 0 for CW
                X = forward * turn * R_foot * math.sin(theta)
                Y = forward * turn * (R - R_foot*math.cos(theta))
                
            elif turn != 0:
                X = turn * W/2 * math.sin(theta)
                Y = turn * W/2 * math.cos(theta)
             
            Q = quaternion_from_euler(0, 0, theta)
            step = AtlasBehaviorStepData()
            
            # One step already exists, so add one to index
            step.step_index = i+1
            
            # Alternate between feet, start with left
            step.foot_index = is_right_foot
            
            #If moving laterally to the left, start with the right foot
            if (lateral > 0):
                step.foot_index = is_left_foot
            
            step.duration = rospy.get_param("Stride_Duration")
            
            step.pose.position.x = X
            step.pose.position.y = Y
            step.pose.position.z = rospy.get_param("Step_Height")
         
            step.pose.orientation.x = Q[0]
            step.pose.orientation.y = Q[1]
            step.pose.orientation.z = Q[2]
            step.pose.orientation.w = Q[3]
            
            step.swing_height = rospy.get_param("Swing_Height")         
            steps.append(step)
        
        # Add final step to bring feet together
        is_right_foot = 1 - steps[-1].foot_index
        is_even = is_right_foot
        # foot = 1 for left, foot = -1 for right
        foot = 1 - 2 * is_right_foot
        
        if turn == 0:
            Y = Y - foot * W
        elif forward != 0:
            
            # R_foot is radius to foot
            R_foot = R + foot * W/2
            #turn > 0 for counter clockwise
            X = forward * turn * R_foot * math.sin(theta)
            Y = forward * turn * (R - R_foot*math.cos(theta))
        else:
            X = turn * W/2 * math.sin(theta)
            Y = turn * W/2 * math.cos(theta)
            
        Q = quaternion_from_euler(0, 0, theta)
        step = AtlasBehaviorStepData()
        step.step_index = len(steps)
        step.foot_index = is_right_foot
        step.duration = rospy.get_param("Stride_Duration")
        step.pose.position.x = X
        step.pose.position.y = Y
        step.pose.position.z = rospy.get_param("Step_Height")
        step.pose.orientation.x = Q[0]
        step.pose.orientation.y = Q[1]
        step.pose.orientation.z = Q[2]
        step.pose.orientation.w = Q[3]
        step.swing_height = rospy.get_param("Swing_Height")
        
        steps.append(step)

        # 0 for full BDI control, 255 for PID control
        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] 
               
        walk_goal = WalkDemoGoal(Header(), WalkDemoGoal.WALK, steps, \
          AtlasBehaviorStepParams(), AtlasBehaviorStandParams(), \
          AtlasBehaviorManipulateParams(),  k_effort )
        
        self.client.send_goal(walk_goal)

            
        self.client.wait_for_result(\
          rospy.Duration(2*rospy.get_param("Stride_Duration") * \
                         len(steps) + 5))
Esempio n. 7
0
    def build_steps(self, forward, lateral, turn):
        L = self.params["Forward Stride Length"]["value"]
        L_lat = self.params["Lateral Stride Length"]["value"]
        R = self.params["Turn Radius"]["value"]
        W = self.params["Stride Width"]["value"]
        X = 0
        Y = 0
        theta = 0
        dTheta = 0

        if forward != 0:
            dTheta = turn * 2 * math.asin(L / (2 * (R + \
            self.params["Stride Width"]["value"]/2)))
        else:
            dTheta = turn * self.params["In Place Turn Size"]["value"]
        steps = []

        if forward != 0:
            dTheta = turn * 2 * math.asin(L / (2 * (R + \
            self.params["Stride Width"]["value"]/2)))
        else:
            dTheta = turn * self.params["In Place Turn Size"]["value"]
        steps = []

        # This home step doesn't currently do anything, but it's a
        # response to bdi not visiting the first step in a trajectory
        # home_step = AtlasBehaviorStepData()

        # If moving right, first dummy step is on the left
        # home_step.foot_index = 1*(lateral < 0)
        # home_step.pose.position.y = 0.1
        # steps.append(home_step)
        prevX = 0
        prevY = 0

        # Builds the sequence of steps needed
        for i in range(self.params["Walk Sequence Length"]["value"]):
            # is_right_foot = 1, when stepping with right
            is_even = i % 2
            is_odd = 1 - is_even
            is_right_foot = is_even
            is_left_foot = is_odd

            # left = 1, right = -1
            foot = 1 - 2 * is_right_foot

            if self.is_static:
                theta = (turn != 0) * dTheta
                if turn == 0:
                    X = (forward != 0) * (forward * L)
                    Y = (lateral != 0) * (is_odd * lateral * L_lat) + \
                        foot * W / 2
                elif forward != 0:
                    # Radius from point to foot (if turning)
                    R_foot = R + foot * W / 2

                    # turn > 0 for CCW, turn < 0 for CW
                    X = forward * turn * R_foot * math.sin(theta)
                    Y = forward * turn * (R - R_foot * math.cos(theta))

                    self.debuginfo("R: " + str(R) + " R_foot:" + \
                    str(R_foot) + " theta: " + str(theta) +  \
                    " math.sin(theta): " + str(math.sin(theta)) + \
                    " math.cos(theta) + " + str(math.cos(theta)))
                elif turn != 0:
                    X = turn * W / 2 * math.sin(theta)
                    Y = turn * W / 2 * math.cos(theta)
            else:
                theta += (turn != 0) * dTheta
                if turn == 0:
                    X = (forward != 0) * (X + forward * L)
                    Y = (lateral != 0) * (Y + is_odd * lateral * L_lat) + \
                        foot * W / 2
                elif forward != 0:
                    # Radius from point to foot (if turning)
                    R_foot = R + foot * W / 2

                    # turn > 0 for CCW, turn < 0 for CW
                    X = forward * turn * R_foot * math.sin(theta)
                    Y = forward * turn * (R - R_foot * math.cos(theta))

                    self.debuginfo("R: " + str(R) + " R_foot:" + \
                    str(R_foot) + " theta: " + str(theta) +  \
                    " math.sin(theta): " + str(math.sin(theta)) + \
                    " math.cos(theta) + " + str(math.cos(theta)))
                elif turn != 0:
                    X = turn * W / 2 * math.sin(theta)
                    Y = turn * W / 2 * math.cos(theta)

            Q = quaternion_from_euler(0, 0, theta)
            step = AtlasBehaviorStepData()

            # One step already exists, so add one to index
            step.step_index = i

            # Alternate between feet, start with left
            step.foot_index = is_right_foot

            #If moving laterally to the left, start with the right foot
            if (lateral > 0):
                step.foot_index = is_left_foot

            step.duration = self.params["Stride Duration"]["value"]

            step.pose.position.x = X
            step.pose.position.y = Y
            step.pose.position.z = self.params["Step Height"]["value"]

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

            step.swing_height = self.params["Swing Height"]["value"]
            steps.append(step)

        # Add final step to bring feet together
        is_right_foot = 1 - steps[-1].foot_index
        is_even = is_right_foot
        # foot = 1 for left, foot = -1 for right
        foot = 1 - 2 * is_right_foot

        if turn == 0:
            Y = Y + foot * W
        elif forward != 0:
            self.debuginfo("R: " + str(R) + " R_foot:" + \
            str(R_foot) + " theta: " + str(theta) +  \
           " math.sin(theta): " + str(math.sin(theta)) + \
           " math.cos(theta) + " + str(math.cos(theta)))

            # R_foot is radius to foot
            R_foot = R + foot * W / 2
            #turn > 0 for counter clockwise
            X = forward * turn * R_foot * math.sin(theta)
            Y = forward * turn * (R - R_foot * math.cos(theta))
        else:
            X = turn * W / 2 * math.sin(theta)
            Y = turn * W / 2 * math.cos(theta)

        Q = quaternion_from_euler(0, 0, theta)
        step = AtlasBehaviorStepData()
        step.step_index = len(steps)
        step.foot_index = is_right_foot
        step.duration = self.params["Stride Duration"]["value"]
        step.pose.position.x = X
        step.pose.position.y = Y
        step.pose.position.z = self.params["Step Height"]["value"]
        step.pose.orientation.x = Q[0]
        step.pose.orientation.y = Q[1]
        step.pose.orientation.z = Q[2]
        step.pose.orientation.w = Q[3]
        step.swing_height = self.params["Swing Height"]["value"]

        steps.append(step)

        return steps
Esempio n. 8
0
    def _build_steps():
        L = 0.15
        L_lat = 0.15
        R = 2
        W = 0.2
        X = 0
        Y = 0
        theta = 0
        dTheta = 0

        steps = []

        # Builds the sequence of steps needed
        for i in range(50):
            is_even = i%2
            is_odd = 1 - is_even
            is_right_foot = is_even
            is_left_foot = is_odd

            # left = 1, right = -1
            foot = 1 - 2 * is_right_foot

            X = (1 != 0) * (X + 1 * L)
            Y = (0 != 0) * (Y + is_odd * 0 * L_lat) + foot * W / 2

            Q = quaternion_from_euler(0, 0, theta)
            step = AtlasBehaviorStepData()

            # One step already exists, so add one to index
            step.step_index = i

            # Alternate between feet, start with left
            step.foot_index = is_right_foot

            step.duration = 0.63

            step.pose.position.x = X
            step.pose.position.y = Y
            step.pose.position.z = 0.0

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

            step.swing_height = 0.1
            steps.append(step)

        # Add final step to bring feet together
        is_right_foot = 1 - steps[-1].foot_index
        is_even = is_right_foot
        # foot = 1 for left, foot = -1 for right
        foot = 1 - 2 * is_right_foot

        Y = Y + foot * W

        Q = quaternion_from_euler(0, 0, theta)
        step = AtlasBehaviorStepData()
        step.step_index = len(steps)
        step.foot_index = is_right_foot
        step.duration = 0.63
        step.pose.position.x = X
        step.pose.position.y = Y
        step.pose.position.z = 0.0
        step.pose.orientation.x = Q[0]
        step.pose.orientation.y = Q[1]
        step.pose.orientation.z = Q[2]
        step.pose.orientation.w = Q[3]
        step.swing_height = 0.1

        steps.append(step)

        return steps
Esempio n. 9
0
    def bridge_qual_1(self):
        s = []
        # Step to first block
        s.append([0.5, 0, 0])
        # Inch forward on first block
        for i in range(3):
            s.append([0.03, 0, 0])
        # Turn toward second block
        for i in range(4):
            s.append([0, 0, 0.196])
        # Step onto second block
        s.append([0.4, 0, 0])
        s.append([0.4, 0, 0])

        # Turn toward third block
        for i in range(4):
            s.append([0, 0, -0.196])

        # Inch forward on second block
        for i in range(3):
            s.append([0.03, 0, 0])

        # Step to third block
        s.append([0.4, 0, 0])
        #Inch forward on third block
        for i in range(4):
            s.append([0.03, -0.01, 0])

        # Turn toward fourth block
        for i in range(4):
            s.append([0, 0, -0.196])

        #Step onto fourth block
        s.append([0.4, 0, 0])
        s.append([0.4, 0, 0])

        # Turn toward goal
        for i in range(4):
            s.append([0, 0, 0.196])

        #Step onto platform
        s.append([0.4, 0, 0])
        s.append([0.4, 0, 0])
        #Walk through the goal
        for i in range(10):
            s.append([0.3, 0, 0])

        steps = []

        home_step = AtlasBehaviorStepData()

        # If moving right, first dummy step is on the left
        home_step.foot_index = 1
        home_step.pose.position.y = -0.1
        steps.append(home_step)
        X = 0
        Y = 0
        W = 0.2
        theta = 0

        # Build trajectory steps
        for i in range(len(s)):
            is_even = i % 2
            is_odd = 1 - is_even
            is_right_foot = is_even
            foot = 1 - 2 * is_even
            turn = 0

            traj = s[i]
            theta += traj[2]
            if traj[2] > 0:
                turn = 1
            elif traj[2] < 0:
                turn = -1
            X += traj[0] * math.sin(3.14 / 2 -
                                    theta) + traj[1] * math.sin(theta)
            Y += traj[0] * math.cos(3.14 / 2 -
                                    theta) + traj[1] * math.cos(theta)
            print("[" + str(X) + ", " + str(Y) + ", " + str(theta))

            Q = quaternion_from_euler(0, 0, theta)
            step = AtlasBehaviorStepData()

            # One step already exists, so add one to index
            step.step_index = i + 1

            # Alternate between feet, start with left
            step.foot_index = is_right_foot

            step.duration = 0.63

            step.pose.position.x = X - foot * W / 2 * math.sin(theta)
            step.pose.position.y = Y + foot * W / 2 * math.cos(theta)
            step.pose.position.z = 0

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

            step.swing_height = 0.3
            steps.append(step)

        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]

        walk_goal = WalkDemoGoal(Header(), WalkDemoGoal.WALK, steps, \
          AtlasBehaviorStepParams(), AtlasBehaviorStandParams(), \
          AtlasBehaviorManipulateParams(),  k_effort )

        self.client.send_goal(walk_goal)

        for step in steps:
            print("foot: " + str(step.foot_index) + \
              " [" + str(step.pose.position.x) + \
              ", " + str(step.pose.position.y) + ", " + str(theta) + "]")
Esempio n. 10
0
    def bridge_qual_1(self):
        s = []
        # Step to first block
        s.append([0.5, 0, 0])
        # Inch forward on first block
        for i in range(3):
            s.append([0.03, 0, 0])
        # Turn toward second block
        for i in range(4):
            s.append([0, 0, 0.196])
        # Step onto second block
        s.append([0.4, 0, 0])
        s.append([0.4, 0, 0])

        # Turn toward third block
        for i in range(4):
            s.append([0, 0, -0.196])
            
        # Inch forward on second block
        for i in range(3):
            s.append([0.03, 0, 0])
            
        # Step to third block
        s.append([0.4, 0, 0])
        #Inch forward on third block
        for i in range(4):
            s.append([0.03, -0.01, 0])
            
        # Turn toward fourth block
        for i in range(4):
            s.append([0, 0, -0.196])
            
        #Step onto fourth block
        s.append([0.4, 0, 0])
        s.append([0.4, 0, 0])
        
        # Turn toward goal 
        for i in range(4):
            s.append([0, 0, 0.196])    
        
        #Step onto platform
        s.append([0.4, 0, 0])
        s.append([0.4, 0, 0])        
        #Walk through the goal
        for i in range(10):
            s.append([0.3, 0, 0])

        steps = []
            
        home_step = AtlasBehaviorStepData()
        
        # If moving right, first dummy step is on the left
        home_step.foot_index = 1
        home_step.pose.position.y = -0.1
        steps.append(home_step)
        X = 0
        Y = 0
        W = 0.2
        theta = 0
        
        # Build trajectory steps
        for i in range(len(s)):
            is_even = i%2
            is_odd = 1-is_even
            is_right_foot = is_even
            foot = 1 - 2*is_even
            turn = 0
           
            traj = s[i]
            theta += traj[2]
            if traj[2] > 0:
                turn = 1
            elif traj[2] < 0:
                turn = -1 
            X += traj[0]*math.sin(3.14/2 - theta) + traj[1]*math.sin(theta) 
            Y += traj[0]*math.cos(3.14/2 - theta) + traj[1]*math.cos(theta)
            print("[" + str(X) + ", " + str(Y) + ", " + str(theta))
            
            Q = quaternion_from_euler(0, 0, theta)
            step = AtlasBehaviorStepData()
            
            # One step already exists, so add one to index
            step.step_index = i+1
            
            # Alternate between feet, start with left
            step.foot_index = is_right_foot
                       
            step.duration = 0.63
            
            step.pose.position.x = X - foot * W/2 * math.sin(theta)
            step.pose.position.y = Y + foot * W/2 * math.cos(theta)
            step.pose.position.z = 0
         
            step.pose.orientation.x = Q[0]
            step.pose.orientation.y = Q[1]
            step.pose.orientation.z = Q[2]
            step.pose.orientation.w = Q[3]
            
            step.swing_height = 0.3
            steps.append(step)
            
        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] 
               
        walk_goal = WalkDemoGoal(Header(), WalkDemoGoal.WALK, steps, \
          AtlasBehaviorStepParams(), AtlasBehaviorStandParams(), \
          AtlasBehaviorManipulateParams(),  k_effort )
        
        self.client.send_goal(walk_goal)
        
        for step in steps:
            print("foot: " + str(step.foot_index) + \
              " [" + str(step.pose.position.x) + \
              ", " + str(step.pose.position.y) + ", " + str(theta) + "]")