Assisted arm navigation package offers similar functionality as the Warehouse Viewer - an interactive (collision free) arm motion planning. It has been designed for Care-O-Bot within the SRS project, but can be easily modified for any robot running the arm_navigation stack. It enables a user to start the arm planning through RVIZ plugin with a simple interface. The goal position of the end effector can be set by a 6 DOF Interactive Marker or by using SpaceNavigator device. For collision free trajectory planning a collision map produced by Environment Model is used.

Current version has been tested only with ROS Electric.

Assisted arm navigation is divided into following packages:

  • srs_assisted_arm_navigation: Main functionality.

  • srs_assisted_arm_navigation_msgs: Definition of services and action interface.

  • srs_assisted_arm_navigation_ui: RVIZ plugin.

For example of how this functionality can be integrated into more complex system, please take a look on srs_arm_navigation_tests, where is the integration into SRS structure in form of SMACH generic states implemented.


This is how it looks in RVIZ when user starts arm planning. There is 6 DOF interactive marker, marker representing arm (green) and marker for the object to be grasped.

Assisted arm navigation started.

Collision with environment or with the object is clearly indicated as well as the situation when desired goal position is out of reach.

Gripper in a collision.

Position is not reachable.

When the trajectory is planned, user can play its animation several times and decide if it's reasonable and safe.

Animation of planned trajectory.

User interface consists of few controls and contains description of the task for user (if using action interface to give tasks to user).

User interface of Assisted arm navigation.

More detailed user interface documentation can be found on its own page.


Assisted arm navigation node communicates with user interface using set of services. There are also some services for adding collision objects etc. The most important is action interface which can be used to ask user to perform some task.

Actionlib interface

Action Subscribed Topics

/but_arm_manip/manual_arm_manip_action/goal (srs_assisted_arm_navigation_msgs/ManualArmManipActionGoal)

  • A task for user.
/but_arm_manip/manual_arm_manip_action/cancel (actionlib_msgs/GoalID)
  • A request to cancel given task.

Action Published Topics

/but_arm_manip/manual_arm_manip_action/feedback (srs_assisted_arm_navigation_msgs/ManualArmManipActionFeedback)

  • Feedback contains the current state of the task.
move_base/status (actionlib_msgs/GoalStatusArray)
  • Provides status information on the goals that are sent to the assisted_arm_navigation action.
/but_arm_manip/manual_arm_manip_action/result (srs_assisted_arm_navigation_msgs/ManualArmManipActionResult)
  • Result contains information about task from user (succeeded, failed etc.).

Topics, services, parameters

Subscribed Topics

/spacenav/joy (sensor_msgs/Joy)
  • tbd
/spacenav/offset (geometry_msgs/Vector3)
  • tbd
/spacenav/rot_offset (geometry_msgs/Vector3)
  • tbd

Published Topics

/but_arm_manip/state (srs_assisted_arm_navigation_msgs/AssistedArmNavigationState)
  • A state of arm navigation.


/but_arm_manip/arm_nav_new (srs_assisted_arm_navigation_msgs/ArmNavNew)
  • Called from user interface, when there is a request for new trajectory planning.
/but_arm_manip/arm_nav_plan (srs_assisted_arm_navigation_msgs/ArmNavPlan)
  • Plan from current position to the goal position.
/but_arm_manip/arm_nav_play (srs_assisted_arm_navigation_msgs/ArmNavPlay)
  • Visualize trajectory.
/but_arm_manip/arm_nav_execute (srs_assisted_arm_navigation_msgs/ArmNavExecute)
  • Execute trajectory.
/but_arm_manip/arm_nav_reset (srs_assisted_arm_navigation_msgs/ArmNavReset)
  • Cancel current planning.
/but_arm_manip/arm_nav_success (srs_assisted_arm_navigation_msgs/ArmNavSuccess)
  • Task was successful (user pressed "Success" button).
/but_arm_manip/arm_nav_failed (srs_assisted_arm_navigation_msgs/ArmNavFailed)
  • User was not able to finish given task.
/but_arm_manip/arm_nav_refresh (srs_assisted_arm_navigation_msgs/ArmNavRefresh)
  • Refresh planning scene.
/but_arm_manip/arm_nav_coll_obj (srs_assisted_arm_navigation_msgs/ArmNavCollObj)
  • Add bounding box of object to the planning scene.
/but_arm_manip/arm_rem_coll_obj (srs_assisted_arm_navigation_msgs/ArmNavRemoveCollObjects)
  • Remove all collision objects.
/but_arm_manip/arm_nav_set_attached (srs_assisted_arm_navigation_msgs/ArmNavSetAttached)
  • Set collision object to be attached.
/but_arm_manip/arm_nav_move_palm_link (srs_assisted_arm_navigation_msgs/ArmNavMovePalmLink)
  • Move virtual end effector to some absolute position.
/but_arm_manip/arm_nav_move_palm_link_rel (srs_assisted_arm_navigation_msgs/ArmNavMovePalmLinkRel)
  • Move virtual end effector relatively.
/but_arm_manip/arm_nav_switch_aco (srs_assisted_arm_navigation_msgs/ArmNavSwitchACO)
  • Enable/disable artificial collision object attached to gripper.
/but_arm_manip/arm_nav_repeat (srs_assisted_arm_navigation_msgs/ArmNavRepeat)
  • User pressed "Repeat" button (if it was allowed by action).
/but_arm_manip/arm_nav_step (srs_assisted_arm_navigation_msgs/ArmNavStep)
  • Undo / redo.
/but_arm_manip/arm_nav_stop (srs_assisted_arm_navigation_msgs/ArmNavStop)
  • Stop execution of trajectory.


~arm_planning/arm_planning/inflate_bb (double, default: "1.0")
  • Bounding box of each object inserted into planning scene will be inflated by this factor.
~arm_planning/world_frame (string, default: "map")
  • Planning will be performed in this coordinate system.
~arm_planning/end_eff_link (string, default: "sdh_palm_link")
  • End effector link.
~arm_planning/joint_controls (bool, default: "false")
  • Enable/disable interactive markers for all joints.
~arm_planning/make_collision_objects_selectable (bool, default: "false")
  • If the object inserted into the planning scene should be selectable or not.
~arm_planning/aco/link (string, default: "arm_7_link")
  • Artificial collision object (when enabled) will be attached to this link.
~arm_planning/aco/default_state (bool, default: "false")
  • Default state of the artificial collision object.
~spacenav/enable_spacenav (bool, default: "true")
  • Enables usage of Space Navigator control.
~spacenav/use_rviz_cam (bool, default: "true")
  • Enables usage of RVIZ camera position to make control of end effector in user perspective. Camera position must be published as TF transformation. There is plugin for publishing this in srs_env_model_ui package.
~spacenav/rviz_cam_link (string, default: "rviz_cam")
  • TF frame for RVIZ camera.
~spacenav/max_val (double, default: "350.0")
  • Maximal value for data from Space Navigator. Higher values will be limited.
~spacenav/min_val_th (double, default: "0.05")
  • Threshold for normalized values (current / max_val). Must be in <0.0, 0.5> range.
~spacenav/step (double, default: "0.1")
  • Step for changes of end effector interactive marker position.
~arm_links (list of strings, default: "cob3-3 arm links")
  • List of arm links.
~set_planning_scene_diff_name (string, default: "environment_server/set_planning_scene_diff") ~left_ik_name (string, default: "cob3_arm_kinematics/get_constraint_aware_ik")
  • Constraint aware IK service.
~planner_1_service_name (string, default: "ompl_planning/plan_kinematic_path")
  • Planner service name.
~trajectory_filter_1_service_name (string, default: "trajectory_filter_server/filter_trajectory_with_constraints")
  • Trajectory filter service name.
~vis_topic_name (string, default: "planning_scene_visualizer_markers")
  • Topic for publishing visualization markers.
~left_ik_link (string, default: "arm_7_link")
  • End effector link for which we perform IK.
~left_arm_group (string, default: "arm")
  • Arm group.
~execute_left_trajectory (string, default: "arm_controller/follow_joint_trajectory")
  • Arm controller action.


  • install COB stacks from Ubuntu repository or from git (see installation instructions)

  • checkout srs and srs_public git repositories
  • compile srs_assisted_arm_navigation package

rosmake srs_assisted_arm_navigation


First, please take a look on how to install and use user interface for assisted arm manipulation.

For starting COB simulation, actionlib server use this:

roslaunch srs_assisted_arm_navigation but_arm_nav_sim.launch

and then next command to start user interface (RVIZ):

roslaunch srs_assisted_arm_navigation_ui rviz.launch

There is an example of a rospy script which can be used to "trigger" manipulation task. Run it and operator should be asked for action by messagebox from RVIZ plugin.

   1 #!/usr/bin/env python
   2 import roslib; roslib.load_manifest('your_package')
   3 import rospy
   4 import actionlib
   6 from srs_assisted_arm_navigation_msgs.msg import *
   8 def main():
  10   rospy.init_node('arm_manip_action_test')
  11   rospy.loginfo("Node for testing actionlib server")
  13   client = actionlib.SimpleActionClient('/but_arm_manip/manual_arm_manip_action',ManualArmManipAction)
  15   rospy.loginfo("Waiting for server...")
  16   client.wait_for_server()
  17   goal = ManualArmManipGoal()
  19   goal.allow_repeat = False
  20   goal.action = "Move arm to arbitrary position"
  21   goal.object_name = ""
  23   client.send_goal(goal)
  25   client.wait_for_result()
  26   rospy.loginfo("I have result!! :-)")
  28   result = client.get_result()
  30   if result.success:
  31     rospy.loginfo("Success!")
  33   if result.failed:
  34     rospy.loginfo("Failed :-(")
  36   rospy.loginfo("Time elapsed: %ss",result.time_elapsed.to_sec())
  38 if __name__ == '__main__':
  39   main()

For instructions on how to test SRS scenarios, please see srs_arm_navigation_tests.

How to adapt it for your robot

Wiki: srs_assisted_arm_navigation (last edited 2013-04-04 11:39:27 by ZdenekMaterna)