How to Use ControlIt! with trikey_pinned (Joint Position)

This section describes how to control the joint position of trikey_pinned using ControlIt!

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 (note that trikey_pinned relies on trikey URDFs):

$ roscd trikey_pinned_controlit_config/models
$ ./generate_trikey_pinned_urdfs.sh

Launch Using a Single Command

To run the demo using a single command:

$ reset && roslaunch trikey_pinned_controlit_config simulate_jpos.launch

Launch Using Multiple Commands

Start roscore:

$ roscore

Start the shared memory manager:

$ rosrun shared_memory_interface shared_memory_manager

Load the parameters onto the ROS parameter server (this command will run then exit):

$ roslaunch trikey_pinned_controlit_config  load_trikey_pinned_parameters.launch

Start the simulation:

$ roslaunch trikey_pinned_controlit_config gazebo.launch

You should now see Gazebo's window with the robot loaded. The simulation will initially be stopped. Leave it this way for now.

Start the controller:

$ roslaunch trikey_pinned_controlit_config start_controlit.launch

The above command will start the ControlIt! controller and block. Hit play in Gazebo to start the demo.

Usage Notes

To change the joint position reference (goal):

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

To visualize the joint position error:

$ rqt_plot /trikey_pinned_controller/JPosTask/error/data[0] /trikey_pinned_controller/JPosTask/error/data[1]  /trikey_pinned_controller/JPosTask/error/data[2]

To change the Kp gains of the joint position task:

$ rostopic pub /trikey_pinned_controller/JPosTask/kp std_msgs/Float64MultiArray '{layout: {dim: [{label: "x", size: 3, stride: 3}], data_offset: 0}, data: [100, 100, 100]}'

To change the Kd gains of the joint position task:

$ rostopic pub /trikey_pinned_controller/JPosTask/kd std_msgs/Float64MultiArray '{layout: {dim: [{label: "x", size: 3, stride: 3}], data_offset: 0}, data: [20, 20, 20]}'

To make the joints follow a square wave goal:

$ rosrun rapid_goal_generators SquareWaveGenerator.py -t /trikey_pinned_controller/JPosTask/goalPosition -p 5