Ingo's ROS code sandbox

You can check out my public Mercurial repository with:

hg clone http://toychest.in.tum.de/users/kresse/sandbox

NOTE: The relevant code is being moved to github and bitbucket. Stay tuned.

Among other stuff it contains:

• a system for constraint based task description:
  • specify constraints between two objects like height, distance, align, …
  • specify a range of allowed values for each constraint
  • orocos realtime component (based on iTaSC)
  • python executive, sending ROS messages to that component
  • a visualization of constraints for rviz, using the task jacobian
• a visualization of twists for rviz
• a pretty general joystick-to-twist converter
• a little tool that makes your speaker (or soundcard) beep tunes from ROS.

command line script 'state' in feature_constraints_utils.

• state get : Print the current values of chi, chi_des, joint_angles, tool_pose and object_pose in the form:

  chi:
  1.1 2.2 -4.44 ...
  joints:
  1 2 3
  ...

  For clarity, the number of decimal digits are limited to 3 and can be set using the rosparam ~digits.

• state get item: Print item in a single line, where item is one of chi, chi_des, joint_angles, tool_pose and object_pose.

• state set angles angles
• state set chi chi-angles
• state set chi - : (repeatedly) get values from stdin

The semantics of chi-angles is as follows: A numeric values translates into a small range around this value, a string becomes 'not important' and sets the respective weight to zero. The command state set angles only accepts numeric values.

• state reset : Stop all feature controllers and resets the arm angles to default values, stored in the rosparam: /loopback_controllers/joints.

• state prefix : Get the current feature controller name which is used as a prefix in topic names.
• state prefix name : Set the prefix. This name is stored in the rosparam '/constraint_controller' .
• state prefixes : Search topic names and display potential prefixes.

Slightly different approach: A rosparam interface.

• state get yaml >file.xml : ouputs the yaml in a format that can be loaded with rosparam load.
• state get param /param_name : directly saves the state into /param_name
• state set chi /param_name/chi
• …

Nothing speaks against supporting both interfaces.

Why not:

• state set _chi:=/param_name/chi ?

This would be too error-prone to use since the value of /state/chi (which is only a name) would stay on the param server and would influence the next call to state. Contrary to ROS conventions, state would have to delete this parameter after reading it.

NOTES:

• Joint angles and poses are taken from /tf and /joint_states.
• The interface to the feature constraint controller is defined by it's name prefix and the topics <prefix>/constraint_config, <prefix>/constraint_command and <prefix>/constraint_state.
• When no prefix is specified, the first feature controller that is found is used.
• Joint angle names are are recovered from the URDF using the package 'urdf_parser_py', using the name of the first and last joint which in turn is taken from the constraint_controller parameters. A fallback for PR2 and Rosie arm joint names is built in, for the case that 'urdf_parser_py' is missing.

TODO:

• When talking to two feature constraint controllers, prefixes must be switched all the time. Supporting an environment variable should be more convenient.
• (Future): What shall we do if two arms and/or several constraint sets are active in the same controller? Give names to arms and constraint sets and use these names with every invocation of state? Have a rosparam, that holds the default name, for convenience? This also becomes important when implementing multi-robot functionality.