How to Use ControlIt! with stickbot_lowerleg_3dof (Joint Position)


Generate the Model

To generate the model:

$ roscd stickbot_lowerleg_3dof_controlit/models/
$ ./

Launch Using a Single Command

To run the demo using a single command:

$ reset && roslaunch stickbot_lowerleg_3dof_controlit simulate_jpos.launch

Launch Using Multiple Commands

Start roscore:

$ roscore

Start the shared memory manager:

$ rosrun shared_memory_interface shared_memory_manager

Load the necessary parameters (this command will run then exit):

$ roslaunch stickbot_lowerleg_3dof_controlit load_stickbot_lowerleg_3dof_parameters.launch

Start the simulation:

$ roslaunch stickbot_lowerleg_3dof_controlit gazebo.launch

Gazebo will appear with a simple 3DOF robot as shown below.

Start the controller:

$ roslaunch stickbot_lowerleg_3dof_controlit start_controlit.launch

The above command will start the controller and block. Hit play in Gazebo to start the demo. You should see the robot stabilize in the following pose:

Start RViz:

$ rosrun rviz rviz

Usage Notes

To change the pose of the robot:

$ rostopic pub --once /stickbot_lowerleg_3dof_controller/JPosTask/goalPosition std_msgs/Float64MultiArray '{layout: {dim: [{label: "x", size: 3, stride: 3}], data_offset: 0}, data: [0.0, 0.3, -0.5]}'

The values above match the robot's initial pose (ankle roll = 0.0, ankle pitch = 0.3, knee = -0.5). The example below will change the reference (goal) angles to be: ankle roll = 0.0 radians, ankle pitch = 0.6 radians, and knee = 0.8 radians.

$ rostopic pub --once /stickbot_lowerleg_3dof_controller/JPosTask/goalPosition std_msgs/Float64MultiArray '{layout: {dim: [{label: "x", size: 3, stride: 3}], data_offset: 0}, data: [0.0, 0.6, 0.8]}' 

Here is what you should see:

Go ahead and play with different reference values to see what happens to the robot.

Under Displays, edit fixed frame from map to lowerLeg.

Next, add a new display (shortcut ctrl+N) and select RobotModel from the rviz folder

Here is what you should see:

Here are some more details about this demo:

  1. The controller's compound task and constraint set is specified by the following YAML file: hcrl_robots/stickbot_lowerleg_3dof/parameters/JointPositionControl.yaml
  2. The gazebo plugin’s source code is here: hcrl_robots/hcrl_simulator/gazebo_plugins/src/SMControlPlugin.cpp
  3. The controller's parameters are specified here: hcrl_robots/stickbot_lowerleg_3dof/launch/load_stickbot_lowerleg_3dof_parameters.launch
  4. The simulation's world file is located here: hcrl_robots/stickbot_lowerleg_3dof/worlds/
    Note the "max_step_size" tag that controls how quickly the simulation runs.
  5. The robot's URDF is located here: hcrl_robots/stickbot_lowerleg_3dof/models/stickbot_lowerleg_3dof/urdf/stickbot_lowerleg_3dof.urdf
  6. We originally used ROS topics for the transport layer but it had high round-trip-time (RTT) latencies.  Thus, we switched to using shared memory transport.