WARNING: This documentation is for Gazebo-classic, which has been superseded by Gazebo. Click here to see the documentation for the latest Gazebo release

Back

Animate joints

Overview

In this tutorial, we'll move a robot in simulation without dynamics through the use of a model plugin that listens to the trajectory_msgs::JointTrajectory message over a ROS topic.

Setup

We assume that you've already done the installation step.

If you haven't done so, add the environment setup.sh files to your .bashrc.

echo 'source /usr/share/drcsim/setup.sh' >> ~/.bashrc
source ~/.bashrc

If you haven't already, create a ros directory in your home directory and add it to your $ROS_PACKAGE_PATH. From the command line

mkdir ~/ros
echo "export ROS_PACKAGE_PATH=\$HOME/ros:\$ROS_PACKAGE_PATH" >> ~/.bashrc
source ~/.bashrc

Use roscreate-pkg to create a ROS package for this tutorial, depending on roscpp and trajectory_msgs:

cd ~/ros
roscreate-pkg joint_animation_tutorial rospy trajectory_msgs std_msgs
cd joint_animation_tutorial
mkdir scripts
cd scripts

Model Plugin Controller

A joint trajectory model plugin has been embedded in the DRC robot. The plugin subscribes to a ROS topic containing JointTrajectory messages, and plays back the joint trajectory on the robot. This is useful when you want to move the robot without worrying about the dynamics of the robot model. This plugin also disables physics updates while playing back the joint trajectory.

Create a ROS Publisher

Download into the current directory a python ROS node that publishes joint trajectory messages joint_animation.py:

wget http://github.com/osrf/gazebo_tutorials/raw/master/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

Make the file executable:

chmod +x joint_animation.py

The Code explained

#!/usr/bin/env python

import roslib; roslib.load_manifest('joint_animation_tutorial')

Standard for every rospy node. This imports roslib and then loads the manifest.xml included in the package so those packages are importable as well.

import rospy, math, time

from trajectory_msgs.msg import 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)

This initializes the node and creates a publisher for the /joint_trajectory topic.

    jt = JointTrajectory()

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

Create an instantiation of a JointTrajectory message and add the time stamp and frame_id to the header.

    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")

Create the list of names of joints that will be controlled.

    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)

Setup a for loop that runs for n=1500 times. It calculates joint angles at two different positions x1 and x2. There should be a position for each joint added above.

        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)

Create a list of positions that the JointTrajectoryPoint will follow. Next, add the JointTrajectoryPoint to the JointTrajectory and proceed to the next point.

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

Log the point that was created.

    pub.publish(jt)
    rospy.spin()

This will publish the single JointTrajectory message, which the robot will execute. The node will then spin, which allows the node to continue running without blocking the CPU.

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

The main method of the rospy node. It prevents the node from executing code if the thread has been shutdown.

Running the Simulation

In terminal, start the DRC robot simulation:

VRC_CHEATS_ENABLED=1 roslaunch drcsim_gazebo atlas.launch

To prevent the robot from falling over (it's not running any controllers), disable gravity by clicking on World->Physics->gravity->z and setting the value to 0.0 or running the following command:
```
gz physics -g 0,0,0
```
To prevent the robot from bouncing off the ground and flying into space (there's no gravity), remove the ground by clicking on World->Models, then right-clicking on ground_plane and clicking Delete.
With gravity off and ground plane deleted, reset the world by clicking Edit->Reset World. The robot should now be in its default pose, "standing" at the origin with arms outstretched.
In a separate terminal:
```
rosrun joint_animation_tutorial joint_animation.py
```
The DRC robot should move according to the published ROS JointTrajectory message.

Atlas v4 and v5

The sample code given above will not work for Atlas v4 and v5 because these later models have different joint names and more joints. To animate Atlas v4/v5 joints, download a modified version of the code. The modified script now contains updated joint names:

    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")

Correspondingly, populate the joint angles:

        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: For Atlas v4/v5, we have to explicity turn off PID control as it interferes with joint trajectory control. In addition, Atlas is set to User mode:

    # turn off pid control so it does not interfere with trajectory control
    rospy.loginfo("Turning off PID control")
    for i in jt.joint_names:
        rospy.set_param('/atlas_controller/gains/' + i[7:] + '/p', 0)
        rospy.set_param('/atlas_controller/gains/' + i[7:] + '/i', 0)
        rospy.set_param('/atlas_controller/gains/' + i[7:] + '/d', 0)

    rospy.sleep(1.0)
    # set Atlas to User mode
    mode_pub = rospy.Publisher('/atlas/control_mode', String, queue_size=1)
    mode_pub.publish(String("ragdoll"))
    rospy.sleep(1.0)
    mode_pub.publish(String("User"))

To run the new joint trajectory publisher, follow steps similar to above, but start DRCSim with the following command to launch Atlas v4:
```
VRC_CHEATS_ENABLED=1 roslaunch drcsim_gazebo atlas.launch model_args:="_v4"
```
Or you can launch Atlas v5:
```
VRC_CHEATS_ENABLED=1 roslaunch drcsim_gazebo atlas.launch model_args:="_v5"
```
Disable gravity by clicking on World->Physics->gravity->z and setting the value to 0.0 or running the following command:
```
gz physics -g 0,0,0
```
Remove the ground by clicking on World->Models, then right-clicking on ground_plane and clicking Delete.
Reset the model poses by clicking Edit->Reset Model Poses.
Finally, in a separate terminal, run the the joint_animation_v4v5.py script:
```
rosrun joint_animation_tutorial joint_animation_v4v5.py
```