moesenle/cram_pr2 — pull request #25 by fairlight1337 (2012-10-19 14:46:45+00:00)
Title: Registering collision objects added taking arm sides into account during FK calculation

The function `register-collision-object' was implemented and now supplies the environment server with collision volumes (at the moment, just cylinders). The function is easily extendable for other types of collision objects. Also, made distance and reaching-length functions take the arm side they are supposed to use into account (before, the FK solver of just the right arm was used and correct reaching length calculations were only conducted for the link `r_wrist_roll_link').
Also, deleted a few old (and unused) functions, refactored code, printouts (now using `ros-info' instead of `format') and fixed line lengths/indentations.