How to use the Atlas Sim Interface
이번 장에서는 어떻게 Atlas가 동적 혹은 정적으로 걷도록 하기 위해 Atlas Sim 인터페이스를 사용 하는지에 대해서 설명한다.
cd ~/catkin_ws/src
catkin_create_pkg atlas_sim_interface_tutorial rospy atlas_msgs
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()
이 노드는 다음을 임폴트 하여야 한다.
#! /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()
위의 파이썬 파일이 실행 되도록 파일 권한을 조정한다.
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
Atlas가 원을 그리며 걷기 시작한다. 동적 걸음에서는 빠르게 정적 걸음에서는 느리게 걷는다.
