How to Use ControlIt! with atlas_plain (COM Position / Chest Orientation)

This section describes how to control atlas_plain and its hands. This is a version of Atlas that does not include the BDI supporting infrastructure.

Generate the URDFs

Note: One only needs to generate the URDFs again if the xacros from which the URDFs are derived have changed. If this is your first time running the demo or you are unsure if the xacros have changed, do not skip this step.

Generate the URDF description files:

$ roscd atlas_plain_controlit/models
$ ./generate_atlas_plain_controlit_urdfs.sh

Launch Automatically Using a Single Command

To run the demo using a single command:

$ reset && roslaunch atlas_plain_controlit simulate.launch

Launch Manually Using Multiple Commands

Start roscore:

$ roscore

Start the shared memory manager:

$ rosrun shared_memory_interface shared_memory_manager

Open a new terminal and load the necessary parameters:

$ roslaunch atlas_plain_controlit load_atlas_plain_parameters_com.launch

The command above will load the parameters and then exit.

Start the simulation:

$ roslaunch atlas_plain_controlit gazebo.launch

Gazebo will appear with Atlas as shown below.

Start the RAPid controller.

$ roslaunch atlas_plain_controlit start_controlit.launch

Hit play in Gazebo to start the controller. The robot should stabilize in an initial pose as shown below. Note that the real-time factor, sim time, real time, and iterations at the bottom of the Gazebo GUI are all non-zero values. This indicates that the simulation is running.

Usage Notes

As you can see, the robot's initial pose has all of its joints at near-zero degrees. This is because setting the initial pose too far away from its all-zero initial state will result in rapid movement that will destabilize the robot and ultimately result in loss of control. We now use a trajectory generator to make the robot go into a “ready” pose (the first two commands are only necessary if your PYTHONPATH environment variable isn't already configured to include wbc_controller_library/src):

$ rosrun atlas_plain_controlit GoToReadyPose_InitialDetected.py -d 2

The robot should now look like:

To close the hands:

$ rosrun atlas_plain_controlit SendSandiaHandCloseCommand.py

The robot's hands should now be closed:

To open its hands:

$ rosrun atlas_plain_controlit SendSandiaHandOpenCommand.py

The robot's hands should now be open.

To view the round-trip-time (RTT) communication latency:

$ rqt_plot /atlas_plain_controller/diagnostics/rttLatency/data

Here's the RTT vs. Time on one of our developers' laptop. It is mostly 3-4ms with spikes up to 20ms:

Here's the RTT vs. Time using another developer's computer. It hovers around 1.8ms with spikes up to 5ms.

To view the servo frequency:

$ rqt_plot /atlas_plain_controller/diagnostics/servoFrequency/data

Here's the servo frequency vs. time on an example computer. It is hovering around 280Hz with spikes going as high as 800Hz.

Here's the servo frequency vs. time on a developer's computer. It is hovering around 800Hz with spikes going as high as 950Hz and has low as 200Hz.

To change the center-of-mass (COM):

$ rostopic pub /atlas_plain_controller/COMTask/goalPosition std_msgs/Float64MultiArray '{layout: {dim: [{label: "x", size: 3, stride: 3}], data_offset: 0}, data: [0.02, 0.0, 0.35]}'

Care must be taken when changing the COM. If you change it too quickly, the feet will slip and the robot will fall. Instead of making one large step, issue small increments to get to the final goal. For example, the sequence of commands below moves the COM forward from x = 0.02m to x = 0.18m. By doing it gradually, the feet will not slip. In the future, we will introduce planners that generate the intermediate points enabling smooth motion from one pose to another.

$ rostopic pub /atlas_plain_controller/COMTask/goalPosition std_msgs/Float64MultiArray '{layout: {dim: [{label: "x", size: 3, stride: 3}], data_offset: 0}, data: [0.05, 0.0, 0.35]}'
$ rostopic pub /atlas_plain_controller/COMTask/goalPosition std_msgs/Float64MultiArray '{layout: {dim: [{label: "x", size: 3, stride: 3}], data_offset: 0}, data: [0.07, 0.0, 0.35]}'
$ rostopic pub /atlas_plain_controller/COMTask/goalPosition std_msgs/Float64MultiArray '{layout: {dim: [{label: "x", size: 3, stride: 3}], data_offset: 0}, data: [0.1, 0.0, 0.35]}'
$ rostopic pub /atlas_plain_controller/COMTask/goalPosition std_msgs/Float64MultiArray '{layout: {dim: [{label: "x", size: 3, stride: 3}], data_offset: 0}, data: [0.15, 0.0, 0.35]}'
$ rostopic pub /atlas_plain_controller/COMTask/goalPosition std_msgs/Float64MultiArray '{layout: {dim: [{label: "x", size: 3, stride: 3}], data_offset: 0}, data: [0.17, 0.0, 0.35]}'
$ rostopic pub /atlas_plain_controller/COMTask/goalPosition std_msgs/Float64MultiArray '{layout: {dim: [{label: "x", size: 3, stride: 3}], data_offset: 0}, data: [0.18, 0.0, 0.35]}'

The robot will now be leaning forward:

Squatting

  1. Launch the simulation, moveit demo, and rapid_locomotion_planner/test_locomotion.launch
  2. rosrun rapid_locomotion_planner squat [squat height, try 0.75 or so]

Here are the commands:

$ roslaunch atlas_plain_controlit simulate.launch
$ roslaunch atlas_plain_moveit_config demo.launch
$ roslaunch rapid_locomotion_planner test_locomotion.launch
$ rosrun rapid_locomotion_planner squat 0.75

Shifting

  1. Launch the simulation, moveit demo, and rapid_locomotion_planner/test_locomotion.launch
  2. rosrun rapid_locomotion_planner go_to_ready
  3. rosrun rapid_locomotion_planner test_locomotion

Here are the commands:

$ roslaunch atlas_plain_controlit simulate.launch
$ roslaunch atlas_plain_config demo.launch
$ roslaunch rapid_locomotion_planner test_locomotion.launch
$ rosrun rapid_locomotion_planner go_to_ready
$ rosrun rapid_locomotion_planner test_locomotion