Modeling Conditional Constraints in MoveIt!
I'm working with a custom robot arm that has some mechanical constraints that are not strictly based around limit-of-motion for any particular joint but rather conditional based on the position of other joints in the arm.
I've walked through the basic panda_arm MoveIt! tutorials and was able to run the "import your own custom arm" tutorial and have it doing basic moves. I can pull the end effector around in the workspace and MoveIt! plans a sane trajectory for it.
I'd like to model more complex behaviors now. On the real arm, it's possible to get a link to telescope out, but only if it has rotated to a specific key position first. There is gearing inside that locks the telescoping part in place until the whole segment rotates into a "keyway" that allows extension(A good analogy is how a bicycle lock works where you rotate the dials into place and then you can pull the pin out of the lock).
I'm developing on Ubuntu 20 w/ ROS Noetic, Gazebo 11 and MoveIt! 1.0
. The current Gazebo model is modeled as a series of links with a revolute and prismatic joint. I have a bit of a hacky joint_state publisher/subscriber system where a node subscribes to the joint states, examines the position of the revolute joint and blocks the associated prismatic joint from moving if the revolute joint is not rotated into one of the "key" positions.
I'm trying to figure out how you would get MoveIt! to know about that in a motion plan. It doesn't look like the constraint examples or systems I've read up on consider this. It seems like there's more focus on keeping tea-cups upright or the end-effector in view, etc., which is fine.
I'm just trying to figure out if there is a combination of Orientation & Positional constraint systems that would allow for me to achieve the kind of behavior I want or if I need to write a custom plugin.
Thanks!
