werner291

Ok, so in `categorize_joints`, there's this bit: ``` if multibody_joints.rigid_body_link(joint.body1).is_some() || multibody_joints.rigid_body_link(joint.body2).is_some() { if !rb1.is_dynamic() || !rb2.is_dynamic() { generic_ground_joints.push(*joint_i); } else { generic_nonground_joints.push(*joint_i); } } ``` The sphere is a...

Possible workaround: attach the dynamic body to a third fixed body (one possibly without a collider), this removes the possibility of a (Fixed multibody part)-Dynamic interaction from the scene. So...

For instance, this test case fails: ``` TEST(FloatingJointTest, distance_rotation_test) { // Create a simple floating joint model with some dummy parameters (these are not used by the test) moveit::core::FloatingJointModel fjm("joint",...

angular_distance_weight_ is just that, I think... the weight given to angular distance; maybe a robot needs to expend more or less energy to rotate in place, versus translate through space....

Btw, possibly related issue #1478

I checked in https://github.com/ompl/ompl/blob/main/src/ompl/geometric/planners/fmt/src/BFMT.cpp, and there do not appear to be any checks when calling `checkMotion` for whether the motion is reversed or not.

Indeed, the direction of motion matters in my case. Basically, the purpose of the motion validator in my case is to ensure that the robot I have only moves in...

However, I do concede that the doc comment on the MotionValidator (https://ompl.kavrakilab.org/classompl_1_1base_1_1MotionValidator.html#a9b6d254308ea12799a72049a8dff3fc6) is not entirely clear on how to deal with forwards or backwards motion. Actually, it's a bit contradictory,...

Notice how RRTConnect does it: https://github.com/ompl/ompl/blob/main/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp#L142 ``` bool validMotion = tgi.start ? si_->checkMotion(nmotion->state, dstate) : si_->isValid(dstate) && si_->checkMotion(dstate, nmotion->state); ``` It specifically reverses the motion validator's arguments if not expanding...

Here's something to test with: ``` #include #include #include #include namespace ob = ompl::base; namespace og = ompl::geometric; class LookForwardValidator : public ompl::base::MotionValidator { public: explicit LookForwardValidator(const ompl::base::SpaceInformationPtr &si) :...