====== 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. {{ :projects:pr2_pancake_pushing.png?300|Constraint specification for pancake-pushing}} 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. ----- ==== Console Interface for feature_constraint_controller === 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 /constraint_config, /constraint_command and /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.