Introduction

이번 장에서는 어떻게 Atlas가 동적 혹은 정적으로 걷도록 하기 위해 Atlas Sim 인터페이스를 사용 하는지에 대해서 설명한다.

Create a ROS Package Workspace

cd ~/catkin_ws/src
catkin_create_pkg atlas_sim_interface_tutorial rospy atlas_msgs

Create a ROS Node

walk.py을 다운로드 하여 **~/ros/atlas_sim_interface_tutorial/src/walk.py**에 저장 한다. 아래 코드 내용을 포함한다.

#! /usr/bin/env python
import roslib; roslib.load_manifest('atlas_sim_interface_tutorial')

from atlas_msgs.msg import AtlasSimInterfaceCommand, AtlasSimInterfaceState, AtlasState
from geometry_msgs.msg import Pose, Point
from std_msgs.msg import String
from tf.transformations import quaternion_from_euler, euler_from_quaternion

import math
import rospy
import sys

class AtlasWalk():

    def walk(self):
        # Initialize atlas mode and atlas_sim_interface_command publishers        
        self.mode = rospy.Publisher('/atlas/mode', String, None, False, \
          True, None, queue_size=1)
        self.asi_command = rospy.Publisher('/atlas/atlas_sim_interface_command', AtlasSimInterfaceCommand, None, False, True, None, queue_size=1)

        # Assume that we are already in BDI Stand mode

        # Initialize some variables before starting.
        self.step_index = 0
        self.is_swaying = False

        # Subscribe to atlas_state and atlas_sim_interface_state topics.
        self.asi_state = rospy.Subscriber('/atlas/atlas_sim_interface_state', AtlasSimInterfaceState, self.asi_state_cb)
        self.atlas_state = rospy.Subscriber('/atlas/atlas_state', AtlasState, self.atlas_state_cb)

        # Walk in circles until shutdown.
        while not rospy.is_shutdown():
            rospy.spin()
        print("Shutting down")

    # /atlas/atlas_sim_interface_state callback. Before publishing a walk command, we need
    # the current robot position
    def asi_state_cb(self, state):
        try:
            x = self.robot_position.x
        except AttributeError:
            self.robot_position = Point()
            self.robot_position.x = state.pos_est.position.x
            self.robot_position.y = state.pos_est.position.y
            self.robot_position.z = state.pos_est.position.z

        if self.is_static:
            self.static(state)
        else:
            self.dynamic(state)

    # /atlas/atlas_state callback. This message provides the orientation of the robot from the torso IMU
    # This will be important if you need to transform your step commands from the robot's local frame to world frame
    def atlas_state_cb(self, state):
        # If you don't reset to harnessed, then you need to get the current orientation
        roll, pitch, yaw = euler_from_quaternion([state.orientation.x, state.orientation.y, state.orientation.z, state.orientation.w])

    # An example of commanding a dynamic walk behavior.     
    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)
        # End of dynamic walk behavior

    # An example of commanding a static walk/step behavior.
    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 not observed
        command.step_params.desired_step.swing_height = 0.1

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

        # Publish a new step command every time a state message is received
        self.asi_command.publish(command)
        # End of static walk behavior

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

        # There will be 60 steps to a circle, and so our position along the circle is current_step
        current_step = step_index % 60

        # yaw angle of robot around circle
        theta = current_step * math.pi / 30

        R = 2 # Radius of turn
        W = 0.3 # Width of stride

        # Negative for inside foot, positive for outside foot
        offset_dir = 1 - 2 * is_left_foot

        # Radius from center of circle to foot
        R_foot = R + offset_dir * W/2

        # X, Y position of foot
        X = R_foot * math.sin(theta)
        Y = (R - R_foot*math.cos(theta))

        # Calculate orientation quaternion
        Q = quaternion_from_euler(0, 0, theta)
        pose = Pose()
        pose.position.x = self.robot_position.x + X
        pose.position.y = self.robot_position.y + Y

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

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

        return pose

if __name__ == '__main__':
    rospy.init_node('walking_tutorial')
    walk = AtlasWalk()
    if len(sys.argv) > 0:
        walk.is_static = (sys.argv[-1] == "static")
    else:
        walk.is_static = False
    walk.walk()

The code explained

이 노드는 다음을 임폴트 하여야 한다.

#! /usr/bin/env python
import roslib; roslib.load_manifest('atlas_sim_interface_tutorial')

from atlas_msgs.msg import AtlasSimInterfaceCommand, AtlasSimInterfaceState, AtlasState
from geometry_msgs.msg import Pose, Point
from std_msgs.msg import String
from tf.transformations import quaternion_from_euler, euler_from_quaternion

import math
import rospy
import sys

이 노드에 대한 퍼블리셔와 서브스크라이버를 초기화 한다. /atlas/atlas_sim_interface_command, /atlas/mode를 퍼블리시한다. /atlas/atlas_sim_interface_state, /atlas/state를 받는다.

#! /usr/bin/env python
import roslib; roslib.load_manifest('atlas_sim_interface_tutorial')

from atlas_msgs.msg import AtlasSimInterfaceCommand, AtlasSimInterfaceState, AtlasState
from geometry_msgs.msg import Pose, Point
from std_msgs.msg import String
from tf.transformations import quaternion_from_euler, euler_from_quaternion

import math
import rospy
import sys

class AtlasWalk():

    def walk(self):
        # Initialize atlas mode and atlas_sim_interface_command publishers        
        self.mode = rospy.Publisher('/atlas/mode', String, None, False, \
          True, None, queue_size=1)
        self.asi_command = rospy.Publisher('/atlas/atlas_sim_interface_command', AtlasSimInterfaceCommand, None, False, True, None, queue_size=1)

        # Assume that we are already in BDI Stand mode

        # Initialize some variables before starting.
        self.step_index = 0
        self.is_swaying = False

        # Subscribe to atlas_state and atlas_sim_interface_state topics.
        self.asi_state = rospy.Subscriber('/atlas/atlas_sim_interface_state', AtlasSimInterfaceState, self.asi_state_cb)
        self.atlas_state = rospy.Subscriber('/atlas/atlas_state', AtlasState, self.atlas_state_cb)

        # Walk in circles until shutdown.
        while not rospy.is_shutdown():
            rospy.spin()
        print("Shutting down")

atlas_sim_interface_state를 콜백하고 이것은 유용한 정보를 제공한다. 로봇의 현제 위치(BID 제어기에 의해 추정된)를 얻을 수 있다. 이 위치는 This is the atlas_sim_interface_state callback. It provides a lot of useful information. We can get the robot's current position (as estimated by the BDI controller). 이 위치는 로컬 스텝 좌표를 글로벌 스텝 좌표로 변환하는데 필요하다.

    # /atlas/atlas_sim_interface_state callback. Before publishing a walk command, we need
    # the current robot position
    def asi_state_cb(self, state):
        try:
            x = self.robot_position.x
        except AttributeError:
            self.robot_position = Point()
            self.robot_position.x = state.pos_est.position.x
            self.robot_position.y = state.pos_est.position.y
            self.robot_position.z = state.pos_est.position.z

        if self.is_static:
            self.static(state)
        else:
            self.dynamic(state)

걸음의 행동에 정적 동적 두가지의 유형이 있다. 동적은 훨씬 빠르지만 발을 놓는 장소가 정확하지 않다. atlas 로봇이 넘어지게 하는 좋지 않은 걷기 위한 지령 생성이 더 많이 나올 수 있다. 정적은 전체 걸음 trajectory가 안정적이다.

    # /atlas/atlas_sim_interface_state callback. Before publishing a walk command, we need
    # the current robot position
    def asi_state_cb(self, state):
        try:
            x = self.robot_position.x
        except AttributeError:
            self.robot_position = Point()
            self.robot_position.x = state.pos_est.position.x
            self.robot_position.y = state.pos_est.position.y
            self.robot_position.z = state.pos_est.position.z

        if self.is_static:
            self.static(state)
        else:
            self.dynamic(state)

만약 로봇이 world frame에서 회전한다면 방향이 걸음걸이를 위치 시키는데 고려 될 필요가 있다. 이 노드는 방향을 사용하지는 않는다.

    # /atlas/atlas_state callback. This message provides the orientation of the robot from the torso IMU
    # This will be important if you need to transform your step commands from the robot's local frame to world frame
    def atlas_state_cb(self, state):
        # If you don't reset to harnessed, then you need to get the current orientation
        roll, pitch, yaw = euler_from_quaternion([state.orientation.x, state.orientation.y, state.orientation.z, state.orientation.w])

이 함수는 로봇이 원으로 동적으로 걷는다. 이것은 next_step_index_needed로 시작하여 어떤 시점에서 4가지 step을 퍼블리시 할 필요가 있다. 이것은 걷기 위한 제어기가 안정된 걷는 trajectory 생성에 도움을 준다. 일부 메세지가 이 걷기 동작에 사용 혹은 실현되지 않는다. 동적 걷기 동작은 장애물이 없는 평평한 표면에서 잘 작동한다.

    # An example of commanding a dynamic walk behavior.     
    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)
        # End of dynamic walk behavior

이것은 정적 걷기 동작의 예이다. This is an example of static walking/step behavior. 한번에 한 단계만 지정하고 다음 단계 메세지를 언제 보낼지 결정하기 위해 상태 메세지에서 step_feedback field를 확인하여야 한다. 전체 걷기 trajectory를 통해 점차적으로 안정된다. 물체 위로 걷거나 스텝을 밟는 데에 이 동작이 필요하다.

    # An example of commanding a static walk/step behavior.
    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 not observed
        command.step_params.desired_step.swing_height = 0.1

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

        # Publish a new step command every time a state message is received
        self.asi_command.publish(command)
        # End of static walk behavior

이 함수는 현재 step_index를 통해 원 주변을 걷는 자세를 계산한다.

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

        # There will be 60 steps to a circle, and so our position along the circle is current_step
        current_step = step_index % 60

        # yaw angle of robot around circle
        theta = current_step * math.pi / 30

        R = 2 # Radius of turn
        W = 0.3 # Width of stride

        # Negative for inside foot, positive for outside foot
        offset_dir = 1 - 2 * is_left_foot

        # Radius from center of circle to foot
        R_foot = R + offset_dir * W/2

        # X, Y position of foot
        X = R_foot * math.sin(theta)
        Y = (R - R_foot*math.cos(theta))

        # Calculate orientation quaternion
        Q = quaternion_from_euler(0, 0, theta)
        pose = Pose()
        pose.position.x = self.robot_position.x + X
        pose.position.y = self.robot_position.y + Y

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

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

        return pose

걷기를 수행하는 메인 함수 이며 이것은 static이 지정되었는지 지정되지 않았는지 확인 합니다.

if __name__ == '__main__':
    rospy.init_node('walking_tutorial')
    walk = AtlasWalk()
    if len(sys.argv) > 0:
        walk.is_static = (sys.argv[-1] == "static")
    else:
        walk.is_static = False
    walk.walk()

Running

위의 파이썬 파일이 실행 되도록 파일 권한을 조정한다.

chmod +x ~/catkin_ws/src/atlas_sim_interface_tutorial/src/walk.py

시뮬레이션을 실행하라.

roslaunch drcsim_gazebo atlas.launch hand_suffix:=_sandia_hands

동적인 걸음을 위해 다음을 수행하라.

Dynamic

rosrun atlas_sim_interface_tutorial walk.py

정적인 걸음을 위해서는 다음을 수행하라.

Static

gz physics -i 60  # update physics iterations from 50 to 60
rosrun atlas_sim_interface_tutorial walk.py static

What you should see

Atlas가 원을 그리며 걷기 시작한다. 동적 걸음에서는 빠르게 정적 걸음에서는 느리게 걷는다.