Skip to content
Permalink
Browse files

Finished boilerplate version 1 with documentation, screenshot, video

  • Loading branch information...
davetcoleman committed Mar 11, 2015
1 parent 8ce497c commit 1e786529e1f4645a3a921264dc1db5d0e52747c3
@@ -8,6 +8,9 @@ find_package(catkin REQUIRED COMPONENTS
hardware_interface
controller_manager
roscpp
control_msgs
trajectory_msgs
actionlib
)

## System dependencies are found with CMake's conventions
@@ -37,10 +40,16 @@ target_link_libraries(myrobot_ros_control
${catkin_LIBRARIES}
)

## Declare a cpp executable
# ROS Node
add_executable(myrobot_node src/myrobot_node.cpp)
target_link_libraries(myrobot_node
myrobot_ros_control
${catkin_LIBRARIES}
)

# Test trajectory generator node
add_executable(myrobot_test_trajectory src/myrobot_test_trajectory.cpp)
target_link_libraries(myrobot_test_trajectory
${catkin_LIBRARIES}
)

@@ -1,2 +1,43 @@
# ros_control_boilerplate
Provides a simple simulation interface and template for setting up a hardware interface for ros_control
# ros_control Boilerplate

Provides a simple simulation interface and template for setting up a hardware interface for ros_control. This boilerplate demonstrates:

- Creating a hardware_interface for multiple joints for use with ros_control
- Position Trajectory Controller
- Control of 2 joints of a simple robot
- Loading configurations with roslaunch and yaml files
- Generating a random trajectory and sending it over an actionlib interface
- Partial support of joint mode switching (but this feature has not been release with ros_control yet)
- Visualization in Rviz

Developed by [Dave Coleman](dav.ee) at the University of Colorado Boulder

<img align="right" src="https://raw.github.com/davetcoleman/ros_control_boilerplate/indigo-devel/resources/screenshot.png" />

## Video Demo

See [YouTube](https://www.youtube.com/watch?v=Tpj2tx9uZ-o) for a very modest video demo.

## Install

This package depends on [gazebo_ros_demos](https://github.com/ros-simulation/gazebo_ros_demos) for its ``rrbot_description`` package, so be sure to ``git clone``` that along with this package and build in your catkin workspace.
## Run
This package is setup to run the "rrbot" two joint revolute-revolute robot demo. To run its ros_control hardware interface, run:
roslaunch ros_control_boilerplate myrobot_hardware.launch
To visualize its published ``/tf`` coordinate transforms in Rviz run:
roslaunch ros_control_boilerplate myrobot_visualize.launch
To send a random, dummy trajectory to execute, run:
roslaunch ros_control_boilerplate myrobot_test_trajectory.launch
## Limitations
- Does not implement joint limits, estops, transmissions, or other fancy new features of ros_contorl
- Does not have any sort of hard realtime code, this depends largely on your platform, kernel, OS, etc
- Only position control is fully implemented, though some code is in place for velocity and effort control
@@ -1,3 +1,10 @@
# Settings for ros_control hardware interface
hardware_interface:
loop_hz: 10 # hz
joints:
- joint1
- joint2

# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
@@ -7,40 +14,20 @@ joint_state_controller:
position_trajectory_controller:
type: position_controllers/JointTrajectoryController
joints:
- myrobot_joint_1
- myrobot_joint_2
- myrobot_joint_3
- myrobot_joint_4
- myrobot_joint_5
- myrobot_joint_6
- joint1
- joint2
constraints:
goal_time: 5.0
#stopped_position_tolerance: 0.4 # Defaults to 0.01
myrobot_joint_1:
trajectory: 0.60
goal: 0.15
myrobot_joint_2:
trajectory: 0.60
goal: 0.15
myrobot_joint_3:
trajectory: 0.60
goal: 0.15
myrobot_joint_4:
trajectory: 0.60
goal: 0.15
myrobot_joint_5:
joint1:
trajectory: 0.60
goal: 0.15
myrobot_joint_6:
joint2:
trajectory: 0.60
goal: 0.15
# gains:
# myrobot_joint_1: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
# myrobot_joint_2: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
# myrobot_joint_3: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
# myrobot_joint_4: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
# myrobot_joint_5: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
# myrobot_joint_6: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
# joint1: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
# joint2: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}

# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
@@ -5,19 +5,17 @@
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />

<!-- Load example URDF -->
<param name="robot_description" command="$(find xacro)/xacro.py '$(find rrbot_description)/urdf/rrbot.xacro'" />

<group ns="myrobot">

<!-- Load hardware interface -->
<node name="myrobot_hardware_interface" pkg="myrobot_ros_control" type="myrobot_node"
output="screen" launch-prefix="$(arg launch_prefix)">
<rosparam>
period: 0.01 # 100 Hz
write_skip: 100 # Write cycle of 100 Hz
</rosparam>
</node>
<node name="myrobot_hardware_interface" pkg="ros_control_boilerplate" type="myrobot_node"
output="screen" launch-prefix="$(arg launch_prefix)"/>

<!-- Load controller settings -->
<rosparam file="$(find myrobot_ros_control)/config/controllers.yaml" command="load"/>
<rosparam file="$(find ros_control_boilerplate)/config/controllers.yaml" command="load"/>

<!-- Load controller manager -->
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
@@ -0,0 +1,20 @@
<launch>

<!-- GDB functionality -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />

<group ns="myrobot">

<!-- Load controller settings -->
<rosparam file="$(find ros_control_boilerplate)/config/controllers.yaml" command="load"/>

<!-- Load hardware interface -->
<node name="myrobot_test_trajectory" pkg="ros_control_boilerplate" type="myrobot_test_trajectory"
output="screen" launch-prefix="$(arg launch_prefix)"/>

</group>

</launch>

@@ -0,0 +1,10 @@
<launch>

<!-- Load example URDF -->
<param name="robot_description" command="$(find xacro)/xacro.py '$(find rrbot_description)/urdf/rrbot.xacro'" />

<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find rrbot_description)/launch/rrbot.rviz"/>

</launch>

@@ -19,11 +19,18 @@
<build_depend>hardware_interface</build_depend>
<build_depend>controller_manager</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_depend>actionlib</build_depend>

<run_depend>hardware_interface</run_depend>
<run_depend>controller_manager</run_depend>
<run_depend>ros_controllers</run_depend>
<run_depend>rrbot_description</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>control_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
<run_depend>actionlib</run_depend>

<export>
</export>
Binary file not shown.
@@ -45,14 +45,15 @@ MyRobotHardwareInterface::MyRobotHardwareInterface(ros::NodeHandle& nh)
: nh_(nh)
, joint_mode_(1) // POSITION
{
// Initialize hardware communication connection
// DO THIS HERE
// Initialize shared memory and interfaces
init();

// Create the controller manager
controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));

// Get period and create timer
nh_.param("loop_hz", loop_hz_, 0.1);
nh_.param("hardware_interface/loop_hz", loop_hz_, 0.1);
ROS_DEBUG_STREAM_NAMED("constructor","Using loop freqency of " << loop_hz_ << " hz");
ros::Duration update_freq = ros::Duration(1.0/loop_hz_);
non_realtime_loop_ = nh_.createTimer(update_freq, &MyRobotHardwareInterface::update, this);

@@ -65,18 +66,12 @@ MyRobotHardwareInterface::~MyRobotHardwareInterface()

void MyRobotHardwareInterface::init()
{
// Get joints to control
XmlRpc::XmlRpcValue joint_list;
nh_.getParam("joints", joint_list);
if (joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
for (int32_t i = 0; i < joint_list.size(); ++i)
{
ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
joint_names_.push_back(static_cast<std::string>(joint_list[i]));
}
else
ROS_ERROR_STREAM_NAMED("temp","joint list type is not type array???");

// Get joint names
nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0)
{
ROS_FATAL_STREAM_NAMED("init","Not joints found on parameter server for controller, did you load the proper yaml file?");
}
num_joints_ = joint_names_.size();

// Resize vectors
@@ -90,26 +85,18 @@ void MyRobotHardwareInterface::init()
// Initialize controller
for (int i = 0; i < num_joints_; ++i)
{
// Create joint name
//joint_names_.push_back("TODO");

ROS_DEBUG_STREAM_NAMED("constructor","Loading joint name: " << joint_names_[i]);

// Create joint state interface
joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i], &joint_velocity_[i], &joint_effort_[i]));

if (i < 6 )
{
// Create velocity joint interface
velocity_joint_interface_.registerHandle(hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),&joint_velocity_command_[i]));
}
else // this is the gripper
{
// Create position joint interface
position_joint_interface_.registerHandle(hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),&joint_position_command_[i]));
}
// Create position joint interface
position_joint_interface_.registerHandle(hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),&joint_position_command_[i]));

// Create velocity joint interface
//velocity_joint_interface_.registerHandle(hardware_interface::JointHandle(
// joint_state_interface_.getHandle(joint_names_[i]),&joint_velocity_command_[i]));

// Create effort joint interface
//effort_joint_interface_.registerHandle(hardware_interface::JointHandle(
@@ -151,23 +138,21 @@ void MyRobotHardwareInterface::write(ros::Duration elapsed_time)
{
case 1: //hardware_interface::MODE_POSITION:
// Position
std::cout << "position mode " << std::endl;
p_error_ = joint_position_command_[i] - joint_position_[i];
// scale the rate it takes to achieve position by a factor that is invariant to the feedback loop
joint_position_[i] += p_error_ * POSITION_STEP_FACTOR / loop_hz_;
break;

case 2: //hardware_interface::MODE_VELOCITY:
std::cout << "velocity mode " << std::endl;
// Position
joint_position_[i] += joint_velocity_[i] * elapsed_time.toSec();

// Velocity
v_error_ = joint_velocity_command_[i] - joint_velocity_[i];
// scale the rate it takes to achieve velocity by a factor that is invariant to the feedback loop
joint_velocity_[i] += v_error_ * VELOCITY_STEP_FACTOR / loop_hz_;

break;

case 3: //hardware_interface::MODE_EFFORT:
ROS_ERROR_STREAM_NAMED("write","Effort not implemented yet");
break;
@@ -43,7 +43,7 @@
int main(int argc, char** argv)
{
ros::init(argc, argv, "myrobot_hardware_interface");
ros::NodeHandle nh("~");
ros::NodeHandle nh;

// NOTE: We run the ROS loop in a separate thread as external calls such
// as service callbacks to load controllers can block the (main) control
Oops, something went wrong.

0 comments on commit 1e78652

Please sign in to comment.
You can’t perform that action at this time.