Animate joints

Overview

이번 장에서는 ROS topic인 trajectory_msgs::JointTrajectory message 를 이용하여 dynamics 없이 시뮬레이션으로 로봇을 움직일 수 있는 방법에 대해서 알아 본다.

아래와 같이 ROS 패키지를 생성한다.

cd ~/catkin_ws/src
catkin_create_pkg joint_animation_tutorial rospy trajectory_msgs std_msgs
cd joint_animation_tutorial
mkdir scripts
cd scripts

Model Plugin Controller

joint trajectory model plugin이 DRC robot에 내장되어 있고 이 플러그인이 JointTrajectory messages를 포함하는 ROS 토픽을 받는다. 그리고 로봇의 조인트를 trajectory에 따라 움직이게 된다. 이것은 로봇 모델의 dynamics를 고려하지 않고 로봇을 움직여 보고자 할 때 유용하다. 이 플러그인은 또한 로봇의 조인트 trajectory를 구동하는 동안 물리적인 업데이트를 하지 않게 한다.

Create a ROS Publisher

현재의 폴더에 joint trajectory messages joint_animation.py를 퍼블리시 하는 파이썬의 ROS 노드를 다운로드 한다.

wget http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_animate_joints/files/joint_animation.py

#!/usr/bin/env python

import roslib; roslib.load_manifest('joint_animation_tutorial')
import rospy, math, time

from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

def jointTrajectoryCommand():
    # Initialize the node
    rospy.init_node('joint_control')

    print rospy.get_rostime().to_sec()
    while rospy.get_rostime().to_sec() == 0.0:
        time.sleep(0.1)
        print rospy.get_rostime().to_sec()

    pub = rospy.Publisher('/joint_trajectory', JointTrajectory, queue_size=10)
    jt = JointTrajectory()

    jt.header.stamp = rospy.Time.now()
    jt.header.frame_id = "atlas::pelvis"

    jt.joint_names.append("atlas::back_lbz" )
    jt.joint_names.append("atlas::back_mby" )
    jt.joint_names.append("atlas::back_ubx" )
    jt.joint_names.append("atlas::neck_ay"  )
    jt.joint_names.append("atlas::l_leg_uhz")
    jt.joint_names.append("atlas::l_leg_mhx")
    jt.joint_names.append("atlas::l_leg_lhy")
    jt.joint_names.append("atlas::l_leg_kny")
    jt.joint_names.append("atlas::l_leg_uay")
    jt.joint_names.append("atlas::l_leg_lax")
    jt.joint_names.append("atlas::r_leg_lax")
    jt.joint_names.append("atlas::r_leg_uay")
    jt.joint_names.append("atlas::r_leg_kny")
    jt.joint_names.append("atlas::r_leg_lhy")
    jt.joint_names.append("atlas::r_leg_mhx")
    jt.joint_names.append("atlas::r_leg_uhz")
    jt.joint_names.append("atlas::l_arm_elx")
    jt.joint_names.append("atlas::l_arm_ely")
    jt.joint_names.append("atlas::l_arm_mwx")
    jt.joint_names.append("atlas::l_arm_shx")
    jt.joint_names.append("atlas::l_arm_usy")
    jt.joint_names.append("atlas::l_arm_uwy")
    jt.joint_names.append("atlas::r_arm_elx")
    jt.joint_names.append("atlas::r_arm_ely")
    jt.joint_names.append("atlas::r_arm_mwx")
    jt.joint_names.append("atlas::r_arm_shx")
    jt.joint_names.append("atlas::r_arm_usy")
    jt.joint_names.append("atlas::r_arm_uwy")

    n = 1500
    dt = 0.01
    rps = 0.05
    for i in range (n):
        p = JointTrajectoryPoint()
        theta = rps*2.0*math.pi*i*dt
        x1 = -0.5*math.sin(2*theta)
        x2 =  0.5*math.sin(1*theta)

        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        jt.points.append(p)

        # set duration
        jt.points[i].time_from_start = rospy.Duration.from_sec(dt)
        rospy.loginfo("test: angles[%d][%f, %f]",n,x1,x2)

    pub.publish(jt)
    rospy.spin()

if __name__ == '__main__':
    try:
        jointTrajectoryCommand()
    except rospy.ROSInterruptException: pass

파일이 실행 될 수 있도록 아래와 같이 파일 권한을 바꾼다.

chmod +x joint_animation.py

The Code explained

#!/usr/bin/env python

import roslib; roslib.load_manifest('joint_animation_tutorial')

이것은 roslib을 불러오고 이 패키지에 포함된 manifest.xml을 로드한다.

import rospy, math, time

from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

더 많은 모듈을 불러오고 JointTrajectory, JointTrajectoryPoint에 대한 메시지 파일을 불러온다.

Import more modules, and import the message file for JointTrajectory and JointTrajectoryPoint.

def jointTrajectoryCommand():
    # Initialize the node
    rospy.init_node('joint_control')

    print rospy.get_rostime().to_sec()
    while rospy.get_rostime().to_sec() == 0.0:
        time.sleep(0.1)
        print rospy.get_rostime().to_sec()

    pub = rospy.Publisher('/joint_trajectory', JointTrajectory, queue_size=10)

이것은 노드를 초기화 하고 joint_trajectory 토픽 퍼블리셔를 생성한다.

    jt = JointTrajectory()

    jt.header.stamp = rospy.Time.now()
    jt.header.frame_id = "atlas::pelvis"

JointTrajectory 메시지의 객체를 생성하고 헤더에 time stamp와 frame_id를 추가한다.

    jt.joint_names.append("atlas::back_lbz" )
    jt.joint_names.append("atlas::back_mby" )
    jt.joint_names.append("atlas::back_ubx" )
    jt.joint_names.append("atlas::neck_ay"  )
    jt.joint_names.append("atlas::l_leg_uhz")
    jt.joint_names.append("atlas::l_leg_mhx")
    jt.joint_names.append("atlas::l_leg_lhy")
    jt.joint_names.append("atlas::l_leg_kny")
    jt.joint_names.append("atlas::l_leg_uay")
    jt.joint_names.append("atlas::l_leg_lax")
    jt.joint_names.append("atlas::r_leg_lax")
    jt.joint_names.append("atlas::r_leg_uay")
    jt.joint_names.append("atlas::r_leg_kny")
    jt.joint_names.append("atlas::r_leg_lhy")
    jt.joint_names.append("atlas::r_leg_mhx")
    jt.joint_names.append("atlas::r_leg_uhz")
    jt.joint_names.append("atlas::l_arm_elx")
    jt.joint_names.append("atlas::l_arm_ely")
    jt.joint_names.append("atlas::l_arm_mwx")
    jt.joint_names.append("atlas::l_arm_shx")
    jt.joint_names.append("atlas::l_arm_usy")
    jt.joint_names.append("atlas::l_arm_uwy")
    jt.joint_names.append("atlas::r_arm_elx")
    jt.joint_names.append("atlas::r_arm_ely")
    jt.joint_names.append("atlas::r_arm_mwx")
    jt.joint_names.append("atlas::r_arm_shx")
    jt.joint_names.append("atlas::r_arm_usy")
    jt.joint_names.append("atlas::r_arm_uwy")

컨트롤 될 joint 이름의 리스트를 생성한다.

    n = 1500
    dt = 0.01
    rps = 0.05
    for i in range (n):
        p = JointTrajectoryPoint()
        theta = rps*2.0*math.pi*i*dt
        x1 = -0.5*math.sin(2*theta)
        x2 =  0.5*math.sin(1*theta)

n=1500으로 수행되도록 루프를 셋업하고 이 루프는 x1, x2의 조인트 각도를 계산한다. 아래에 추가된 각 조인트의 위치가 될 것이다.

        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        jt.points.append(p)

JointTrajectoryPoint가 따라갈 위치의 리스트를 생성한다. 그리고 JointTrajectoryPoint를 JointTrajectory에 추가한다. 그리고 다음 포인트를 진행한다.

        # set duration
        jt.points[i].time_from_start = rospy.Duration.from_sec(dt)
        rospy.loginfo("test: angles[%d][%f, %f]",n,x1,x2)

생성된 포인트를 로그로 남긴다.

    pub.publish(jt)
    rospy.spin()

이것은 로봇을 움직일 하나의 JointTrajectory 메시지를 퍼블리시 할 것이다. 이 노드는 계속해서 반복 수행된다.

if __name__ == '__main__':
    try:
        jointTrajectoryCommand()
    except rospy.ROSInterruptException: pass

rospy 노드의 메인 함수이고 만약 쓰레드가 다운되면 실행 코드로 부터 노드를 보호한다.

Running the Simulation

1. 터미널 창에서 DRC 로봇 시뮬레이션을 시작한다.

   VRC_CHEATS_ENABLED=1 roslaunch drcsim_gazebo atlas.launch
   

2. 로봇이 넘어지는 것을 방지 하기 위해(어떤 제어기들도 작동되지 않고 있다) World->Physics->gravity->z을 클릭하고 값을 0.0으로 셋팅 해서 중력을 없앤다. 혹은 다음의 명령어를 수행한다.

   gz physics -g 0,0,0
   

3. 로봇이 지면에서 튀기거나 하늘에 떠있는 것을 방지하기 위해(중력이 없다) World->Models을 클릭하고 ground_plane을 오른쪽 클릭해서 삭제를 클릭한다.

4. 중력을 없애고 ground plane을 삭제한다. Edit->Reset World을 클릭해서 world를 리셋한다. 로봇은 이제 팔을 펴고 원점에서 서있는 초기 자세가 될 것이다.

5. 다른 터미널 창을 열고 아래를 수행한다.

   rosrun joint_animation_tutorial joint_animation.py
   

   DRC 로봇은 퍼블리시된 ROS JointTrajectory 메시지에 따라 움직일 것이다.

Atlas v4, v5

이 샘플코드는 Atlas v4와 v5에서는 작동되지 않는다. 왜냐하면 이 모델들은 다른 조인트의 이름과 더 많은 조인트들을 가지고 있기 때문이다. Atlas v4, v5 조인트들을 움직이기 위해서는 modified version를 다운 받아야 한다. 이 수정된 코드는 업데이트 된 조인트의 이름을 포함한다.

    jt.joint_names.append("atlas::back_bkz" )
    jt.joint_names.append("atlas::back_bky" )
    jt.joint_names.append("atlas::back_bkx" )
    jt.joint_names.append("atlas::neck_ry"  )
    jt.joint_names.append("atlas::l_leg_hpz")
    jt.joint_names.append("atlas::l_leg_hpx")
    jt.joint_names.append("atlas::l_leg_hpy")
    jt.joint_names.append("atlas::l_leg_kny")
    jt.joint_names.append("atlas::l_leg_aky")
    jt.joint_names.append("atlas::l_leg_akx")
    jt.joint_names.append("atlas::r_leg_akx")
    jt.joint_names.append("atlas::r_leg_aky")
    jt.joint_names.append("atlas::r_leg_kny")
    jt.joint_names.append("atlas::r_leg_hpy")
    jt.joint_names.append("atlas::r_leg_hpx")
    jt.joint_names.append("atlas::r_leg_hpz")
    jt.joint_names.append("atlas::l_arm_elx")
    jt.joint_names.append("atlas::l_arm_ely")
    jt.joint_names.append("atlas::l_arm_shz")
    jt.joint_names.append("atlas::l_arm_shx")
    jt.joint_names.append("atlas::l_arm_wry")
    jt.joint_names.append("atlas::l_arm_wrx")
    jt.joint_names.append("atlas::l_arm_wry2")
    jt.joint_names.append("atlas::r_arm_elx")
    jt.joint_names.append("atlas::r_arm_ely")
    jt.joint_names.append("atlas::r_arm_shz")
    jt.joint_names.append("atlas::r_arm_shx")
    jt.joint_names.append("atlas::r_arm_wry")
    jt.joint_names.append("atlas::r_arm_wrx")
    jt.joint_names.append("atlas::r_arm_wry2")

해당 조인트 각도들을 생성한다.

        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x2)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x1)
        p.positions.append(x1)
        jt.points.append(p)

Note: Atlas v4/v5에서 joint trajectory control 인터페이스를 가지기 때문에 PID 컨트롤을 꺼야 한다. 게다가 Atlas는 User 모드로 셋팅이 된다.

1. 새로운 joint trajectory 퍼블리셔를 수행하기 위해, 먼저 했던 유사한 절ㅊ라르 따른다. Atlas v4를 launch하기 위해 다음 명령어를 수행한다.

   VRC_CHEATS_ENABLED=1 roslaunch drcsim_gazebo atlas.launch model_args:="_v4"
   

   Atlas v5를 실행하고자 한다면 아래의 명령어로 실행 가능하다.

   VRC_CHEATS_ENABLED=1 roslaunch drcsim_gazebo atlas.launch model_args:="_v5"
   

2. World->Physics->gravity->z를 클릭 하여 0.0으로 값을 설정하면 중력을 없앨 수 있다. 아니면 아래의 명령어로 수행 가능하다.

   gz physics -g 0,0,0
   

3. ground를 삭제한다. World->Models을 클릭하고 ground_plane 오른쪽 클릭하여 삭제를 클릭한다.`

4. Edit->Reset Model Poses를 클릭하여 모델의 자세를 초기화 한다.

5. 마지막으로 다른 터미널창에서 joint_animation_v4v5.py를 실행한다.

   rosrun joint_animation_tutorial joint_animation_v4v5.py