How can I weight the distance metric for MoveIt?

Question

I'm working with a 3-link robot model with a TRR kinematic arrangement. The translation axis is 1.2 m long with limts [0, 1.2]. The rotary axes are 0.4 m long each and have limits of [-pi, pi]. A distance metric of ||q||, while valid, will obviously move the translation axis quite a bit relative to the rotary axes. This is because a distance of 1 m is treated equivalent to a rotation of 1 radian. What I'd like is to assign weights to the distance metric so that I can define the "aspect ratio" of the C-space to be much more sane - but I can't find a method to do this.

I see in OMPL that for CompoundStateSpace a method exists (setSubSpaceWeight) that accomplishes this. I'm looking for similar functionality through some configuration of the MoveIt state space. Possible?

---

Originally posted by philip.freeman on ROS Answers with karma: 11 on 2019-04-22

Post score: 1

Answer 1

You can modify the distance metric, but not without modifying MoveIt itself.

You should be able to call ModelBasedStateSpace::setDistanceFunction() with your preferred distance function. OMPL delegates it's distance calls there, so that will change the distance metric for the planner you're using. You'll probably want to call setDistanceFunction somewhere in the presolve function.

We're working on making this simpler by allowing you to write your own planning plugin for OMPL, I'll update this answer when that progresses.

---

Originally posted by BryceWilley with karma: 711 on 2019-04-22

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by philip.freeman on 2019-04-23:
Why would writing my own planning plugin for OMPL make this easier? I don't really want to change the planning algorithm, I just want to set the distance function (as you note). Wouldn't exposing an interface to MoveIt's setDistanceFunction be the preferred solution?

Comment by BryceWilley on 2019-04-23:
The plugin would make things easier because you'd be able to write a very light wrapper around the OMPL planner plugin without modifying the MoveIt source itself.

It might be possible to expose an interface to setDistanceFunction, but all of those OMPL planner classes are hidden behind the Planner Manager interface, which is relatively fixed. You also wouldn't be able to the specific OMPL state structure, which you'd need to do your "aspect ratio" approach.

Feel free to open an issue if this is something you want exposed though.