Wiki

Clone wiki

misc / Home

Setup Instructions

Setting Up your Machine for the R2 Gazebo Simulator:

If you already have Ubuntu 12.04 and ROS Fuerte, skip to Pulling Down Our Code.

Create ros User Group

$ sudo groupadd ros
$ sudo usermod -a -G ros $USER 

Then logout and login again before continuing.

Install ROS and Orocos

Installing ROS On Ubuntu

If you're using an Ubuntu-based system, ROS installation is easy. The following assumes you're using Ubuntu 12.04 Precise, adjust accordingly. Below, the instructions say to install "ros-fuerte-desktop" which is a feature-full version of ROS. If you prefer the bare-bones install without all the graphical tools, substitute "ros-fuerte-ros-base" instead.

$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list'
$ wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
$ sudo apt-get update

If you're not worried about space considerations, then go ahead and install all of ROS:

$ sudo apt-get --force-yes -y --allow-unauthenticated install ros-fuerte-desktop 

If you are concerned about space, then the base package (plus one more) will work just fine with our current usage:

$ sudo apt-get --force-yes -y --allow-unauthenticated install ros-fuerte-ros-comm ros-fuerte-robot-model ros-fuerte-common 

You'll want to go ahead and setup your environment to handle ROS stuff (this will be added to your .bashrc later):

$ source /opt/ros/fuerte/setup.bash 

Give recursive ownership of the /opt/ros directory:

$ sudo chown -R $USER:ros /opt/ros

Finally, get 'rosdep' up and running correctly with our own NASA rosdep definitions.

$ sudo apt-get install python-rosdep python-rosinstall
$ sudo rosdep init
$ sudo sh -c 'echo "yaml https://bitbucket.org/nasa_ros_pkg/nasa_rosdep/raw/master/nasa.yaml" > /etc/ros/rosdep/sources.list.d/30-nasa.list'
$ sudo chmod a+rx /etc/ros
$ sudo chmod a+rx /etc/ros/rosdep
$ sudo chmod a+rx /etc/ros/rosdep/sources.list.d
$ sudo chmod a+r /etc/ros/rosdep/sources.list.d/*.list
$ rosdep update 

Installing the Orocos Stack

$ sudo apt-get --force-yes -y --allow-unauthenticated install libreadline-dev omniorb omniidl omniorb-nameserver libomniorb4-1 libomniorb4-dev libomnithread3-dev libomnithread3c2 gccxml antlr libantlr-dev libxslt1-dev liblua5.1-0-dev ruby1.8-dev libruby1.8 rubygems1.8 libgv-lua
$ sudo apt-get --force-yes -y --allow-unauthenticated install ros-fuerte-orocos-toolchain ros-fuerte-rtt-ros-integration ros-fuerte-rtt-common-msgs ros-fuerte-rtt-ros-comm ros-fuerte-rtt-geometry
$ sudo mkdir /opt/orocos
$ sudo chown $USER:ros /opt/orocos
$ cd /opt/orocos
$ export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/opt/orocos
$ git clone http://pintsize.jsc.nasa.gov/stash/scm/nc/rfsm.git
$ sudo chown -R $USER:ros rfsm  

Pulling Down Our Code

Go to your home directory and grab our misc files from nasa_ros_pkg on Bitbucket with the following command:

$ git clone https://bitbucket.org/nasa_ros_pkg/misc.git 

nasa_ros_pkg on Bitbucket: (https://bitbucket.org/nasa_ros_pkg), 'misc' folder: (https://bitbucket.org/nasa_ros_pkg/misc)

Pull out the rosinstall file and use rosinstall to pull down the rest of nasa_ros_pkg:

$ cd misc
$ mv nasa-ros-pkg.rosinstall ..
$ cd ..
$ rosinstall ~/nasa_ros_pkg ~/nasa-ros-pkg.rosinstall 

Once you have all the files, source the setup.bash file.

$ source ~/nasa_ros_pkg/setup.bash 

Installing Items for Gazebo

If you already have Ubuntu 12.04 and ROS Fuerte configured and running, continue on.

# you should already have the simulator stacks, check this by
$ roscd nasa_r2_simulator
# if you go to a valid directory, make sure you have the latest
$ git pull
# grab dependencies
$ sudo apt-get install ros-fuerte-control ros-fuerte-pr2-simulator ros-fuerte-pr2-controllers swig libstdc++5
# resolve dependencies
$ rosdep install nasa_r2_simulator
# make our stacks
$ rosmake nasa_r2_simulator 

You're done!

Starting up R2 Gazebo

Rosmake and Xml Files

This should have happened in the previous step when rosmaking nasa_r2_simulatory, but be sure all stacks inside of nasa_ros_pkg are made (nasa_common, nasa_r2_common, nasa_r2_simulator, and nasa_robodyn). Double check inside nasa_r2_common/r2_description/robots. When rosmaking, .xml files should have been generated from the .xacro files inside this directory. It should look like this:

proto_legs_control.urdf.xml        r2c_legs.sim.urdf.xml
r2c_full_body_ISS.sim.urdf.xacro   r2c_legs.urdf.xacro
r2c_full_body_ISS.sim.urdf.xml     r2c_legs.urdf.xml
r2c_full_body_no_world.urdf.xacro  r2c_torso.sim.urdf.xacro
r2c_full_body_no_world.urdf.xml    r2c_torso.sim.urdf.xml
r2c_full_body.sim.urdf.xacro       r2.display.urdf.xacro
r2c_full_body.sim.urdf.xml         r2.display.urdf.xml
r2c_full_body.urdf.xacro           r2_orocos.sim.urdf.xacro
r2c_full_body.urdf.xml             r2_orocos.sim.urdf.xml
r2c_legs_control.urdf.xacro        r2.sim.urdf.xacro
r2c_legs_control.urdf.xml          r2.sim.urdf.xml
r2c_legs.display.urdf.xacro        r2.srdf
r2c_legs.display.urdf.xml          r2.urdf.xacro
r2c_legs.sim.urdf.xacro            r2.urdf.xml 

If you don't see these .xml files, try doing rosmake on nasa_r2_simulator again. If this still doesn't work, there's a file inside nasa_r2_common/r2_description called "generateXml.sh". Run this to generate the files:

//Only if you need to generate the xml files inside nasa_r2_common/r2_description/robots
$ roscd r2_description
$ /bin/bash generateXml.sh 

The .xml files should be inside your r2_description/robots folder now.

If everything is made and you have the .xml files, you can launch R2 Gazebo.

Launching Gazebo

Make sure that your graphic drivers are compliant with Gazebo. Check that the driver versions match, or activate an updated/recommended driver before trying to do roslaunch.

There are many different launch scripts to start the robot up in different configurations and in different worlds:

# empty worlds
$ roslaunch r2_gazebo empty_world.launch
$ roslaunch r2_gazebo empty_world_no_gravity.launch 
$ roslaunch r2_gazebo empty_ISS.launch
$ roslaunch r2_gazebo empty_ISS_simple.launch
$ roslaunch r2_gazebo taskboard_only.launch 
# full robot
$ roslaunch r2_gazebo r2c_full_controller.launch # empty world, no gravity
$ roslaunch r2_gazebo r2c_full_ISS_simple_controller.launch #ISS simple 
# upper body
$ roslaunch r2_gazebo r2_orocos_controller.launch # upper body, no stand
$ roslaunch r2_gazebo r2c_torso_controller.launch # upper body with stand and gravity
# Some trouble with table and taskboard. If it seems like some things are missing, relaunch Gazebo
$ roslaunch r2_gazebo r2c_torso_table_controller.launch # upper body on stand in front of table
$ roslaunch r2_gazebo r2c_torso_taskboard_controller.launch # upper body on stand empty world with taskboard 
#legs
$ roslaunch r2_gazebo r2c_legs_controller.launch # legs on stand 
#rviz: These files do not bring up r2_gazebo, they bring up Rviz.
$ roslaunch r2_gazebo r2_display_controller.launch
$ roslaunch r2_gazebo r2c_legs_display_controller.launch 

Controlling Robonaut in R2 Gazebo

Joint Movement

For joint movement, you need to first set the joint settings, and then you will be able to send positions.

With joint movements, the important ROS Topics and Messages are:

Topics:

  • /joint_states (specifically, /joint_states/name. You can find the names for the joints in this array)
  • /joint_ref_settings (where you will send LabeledControllerJointSettings messages)
  • /joint_refs (where you will send LabeledJointTrajectory messages)

Messages

  • nasa_r2_common_msgs/LabeledControllerJointSettings (Message to set Joint limits)
  • nasa_r2_common_msgs/LabeledJointTrajectory (Message to send to move joints)

For example, if the user wants to move Joint 3 in the left arm, the following would need to be done.

For more information on the fields of the ROS messages, see Important ROS Messages

Settings

First, launch R2 Gazebo.

Then, set the limits with LabeledControllerJointSettings. The format for rostopic pub is rostopic pub <topic> <msg_type> <args>. With everything installed, you should be able to tab-complete the arguments once you have specified the topic and msg_type. Then scroll up to the values and change them to the values desired. The topic to send LabeledControllerJointSettings messages over is /joint_ref_settings.

This is just an example value. The limits can be set to other values.

$ rostopic pub /joint_ref_settings nasa_r2_common_msgs/LabeledControllerJointSettings "originator: ''
joint_names:
- '/r2/left_arm/joint3'
jointVelocityLimits:
- 1
jointAccelerationLimits:
- 1"  

Hit enter, and you should see an information message in the same terminal where you launched Gazebo from. It should look something like:

[ INFO] [1405031828.986007313, 15.535000000]: in JointSettingsCallback 

Ctrl-C to get your prompt back in the same terminal you sent the joint settings.

Movement

Now, you can use LabeledJointTrajectory to move joints. This time, use /joint_refs to send the positions. Remember, values are in radians.

$ rostopic pub /joint_refs nasa_r2_common_msgs/LabeledJointTrajectory "header:  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
originator: ''
joint_names:
- '/r2/left_arm/joint3'
points:
- positions:
  - .25
  velocities:
  - 0
  accelerations:
  - 0
  time_from_start:
    secs: 0
    nsecs: 0" 

Again, in your Gazebo window, you should see information messages pop up along the lines of:

[ INFO] [1405031853.952188752, 40.501000000]: Joint Ref Trajectory received
[ INFO] [1405031853.952312516, 40.501000000]: added joint waypoints
[ INFO] [1405031853.952672348, 40.501000000]: wrote status
[ INFO] [1405031853.985146173, 40.534000000]: wrote status
[ INFO] [1405031858.118952986, 44.668000000]: wrote status

Once the joint settings are set for a gazebo instance, you can continue to send joint positions to the simulation without having to send the settings message again.

Cartesian Movement

Once again, to move anything, you need to set the settings first and then the robot can be moved inside of R2 Gazebo.

The important ROS Topics and Messages for Cartesian Movement are the following:

Topics:

  • /pose_states (specifically, /pose_states/name. You can find the names for the frames in this array)
  • /pose_ref_settings (this is the topic where you send settings)
  • /pose_refs (this is the topic you send messages to for cartesian movement)

Messages:

  • nasa_r2_common_msgs/LabeledControllerPoseSettings (Message with all settings)
  • nasa_r2_common_msgs/LabeledPoseTrajectory (Message with movements)

In this example, we will move the lower right arm.

For more information on the fields of the ROS messages, see Important ROS Messages

Settings

First, launch R2 Gazebo.

Then, set the limits with LabeledControllerPoseSettings. The format for rostopic pub is rostopic pub <topic> <msg_type> <args>. With everything installed, you should be able to tab-complete the arguments once you have specified the topic and msg_type. Then scroll up to the values and change them to the values desired. The topic to send LabeledControllerPoseSettings messages over is /pose_ref_settings.

This is just an example value. The maxes can be set to other values.

$ rostopic pub /pose_ref_settings nasa_r2_common_msgs/LabeledControllerPoseSettings "{originator: '', maxLinearVelocity: 1, maxRotationalVelocity: 1, maxLinearAcceleration: 1,
  maxRotationalAcceleration: 1}" 

Hit enter, and you should see an information message in the same terminal where you launched Gazebo from. It should look something like:

[ INFO] [1405032764.724141079, 951.002000000]: in poseSettingsCallback 

Ctrl-C to get your prompt back in the same terminal you sent the pose settings.

Movement

Now, you can use LabeledPoseTrajectory to send Cartesian movements to the robot. This time, use /pose_refs to send the Cartesian movements. Remember, values are in radians.

For axis_priorities, when you tab-complete, it will only have ' ' in the field. Every node needs an array with 6 values, as shown below.

$ rostopic pub /pose_refs nasa_r2_common_msgs/LabeledPoseTrajectory "header:
  seq: 0
  stamp: {secs: 0, nsecs: 0}
  frame_id: ''
originator: ''
nodes: ['/r2/right_lower_arm']
node_priorities:
- {axis_priorities: [1, 1, 1, 1, 1, 1]}
refFrames: ['/r2/right_lower_arm']
points:
- positions:
  - position: {x: 0.1, y: 0.0, z: 0.0}
    orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
  velocities:
  - linear: {x: 0.0, y: 0.0, z: 0.0}
    angular: {x: 0.0, y: 0.0, z: 0.0}
  accelerations:
  - linear: {x: 0.0, y: 0.0, z: 0.0}
    angular: {x: 0.0, y: 0.0, z: 0.0}
  time_from_start: {secs: 0, nsecs: 0}" 

Again, in your Gazebo window, you should see information messages pop up along the lines of:

[ INFO] [1405034501.186245372, 1109.535000000]: got labeled pose traj
[ INFO] [1405034501.186334686, 1109.536000000]: wrote posetraj
[ INFO] [1405034501.217599053, 1109.567000000]: in poseRefsCallback
[ INFO] [1405034501.217640205, 1109.567000000]: Adding cartesian trajectory
[ INFO] [1405034501.218108602, 1109.567000000]: wrote status
[ INFO] [1405034501.254039679, 1109.603000000]: wrote status 

Once the pose settings are set for a gazebo instance, you can continue to send movements to the simulation without having to send the settings message again.

Important ROS Messages

A brief summary of the important ROS Messages, including their name, description, and any applicable variables.

Variables listed as <type> <name>, with applicable explanations in italics.

nasa_r2_common_msgs/LabeledControllerPoseSettings

Pose settings for controller. These need to be set before sending Cartesian movements with LabeledPoseTrajectory.

Variables:

  • string originator
  • float64 maxLinearVelocity Maximum Linear Velocity (Limit for Cartesian movement)
  • float64 maxRotationalVelocity Maximum Rotational Velocity (Limit for Cartesian movement)
  • float64 maxLinearAcceleration MaximumLinearAcceleration (Limit for Cartesian movement)
  • float64 maxRotationalAcceleration Maximum Rotational Acceleration (Limit for Cartesian movement)

nasa_r2_common_msgs/LabeledControllerJointSettings

Joint settings for controller. These need to be set before sending positions with LabeledJointTrajectory.

Variables:

  • string originator
  • string[] joint_names Specify the name of the joint you want to set limits on. The entire name of the joint must be specified, i.e. /r2/left_arm/joint3. The index for a joint in this string array will correspond to the joint's index in the limit arrays.
  • float64[] jointVelocityLimits Set the velocity limits for a specified joint. Example value: 1
  • float64[] jointAccelerationLimits Set the acceleration limits for a specified joints. Example value: 1

nasa_r2_common_msgs/LabeledJointTrajectory

Trajectory for Joints. Edit these for Joint movement.

Variables:

  • std_msgs/Header header
    • uint32 seq
    • time stamp
    • string frame_id
  • string originator
  • string[] joint_names Specify the name of the joint you'd like to move. The entire name of the joint must be specified, i.e. /r2/left_arm/joint3. The index for a joint in this string array will correspond to the joint's index in the goal point array.
  • trajectory_msgs/JointTrajectoryPoint[] points The goal point for the joint. Generally, you only need to edit the position of the joint. Values are in radians.
  • float64[] positions
  • float64[] velocities
  • float64[] accelerations
  • duration time_from_start

nasa_r2_common_msgs/LabeledPoseTrajectory

Trajectory for Poses. Edit these for Cartesian movement.

Variables:

  • std_msgs/Header header
    • uint32 seq
    • time stamp
    • string frame_id
  • string originator
  • string[] nodes The node specified is the frame that will be controlled. Each node has a priority for each axis (defined below under node_priorities)
  • nasa_r2_common_msgs/PriorityArray[] node_priorities Priorities that are assigned to each axis of a node NOTE: When specifying priorities, each node needs an array of 6 numbers, ex: [1, 1, 1, 1, 1, 1]
  • uint8 IGNORE=0
  • uint8 CRITICAL=1
  • uint8 HIGH=2
  • uint8 MEDIUM=3
  • uint8 LOW=4
  • uint8 OPT=5
  • uint8[] axis_priorities
  • string[] refFrames The reference frame for each node
  • nasa_r2_common_msgs/PoseTrajectoryPoint[] points Each node has a goal point, which is defined here.
  • geometry_msgs/Pose[] positions Set Position attributes
    • geometry_msgs/Point position Set the x, y, and z values for point position.
      • float64 x
      • float64 y
      • float64 z
    • geometry_msgs/Quaternion orientation Set the x, y, z, and w values for the quaternion orientation.
      • float64 x
      • float64 y
      • float64 z
      • float64 w
  • geometry_msgs/Twist[] velocities Set Velocity attributes
    • geometry_msgs/Vector3 linear Set the x, y, and z values for the linear velocity
      • float64 x
      • float64 y
      • float64 z
    • geometry_msgs/Vector3 angular Set the x, y, and z values for the angular velocity
      • float64 x
      • float64 y
      • float64 z
  • geometry_msgs/Twist[] accelerations Set Acceleration attributes
    • geometry_msgs/Vector3 linear Set the x, y, and z values for the linear acceleration
      • float64 x
      • float64 y
      • float64 z
    • geometry_msgs/Vector3 angular Set the x, y, and z values for the angular acceleration
      • float64 x
      • float64 y
      • float64 z
  • duration time_from_start

Updated