diff --git a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h index d9d1a8d48a..a5bca1b0a4 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h @@ -46,6 +46,13 @@ namespace core class PlanarJointModel : public JointModel { public: + /** \brief different types of planar joints we support */ + enum MotionModel + { + HOLONOMIC, // default + DIFF_DRIVE + }; + PlanarJointModel(const std::string& name); void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override; @@ -75,6 +82,26 @@ class PlanarJointModel : public JointModel angular_distance_weight_ = weight; } + double getMinTranslationalDistance() const + { + return min_translational_distance_; + } + + void setMinTranslationalDistance(double min_translational_distance) + { + min_translational_distance_ = min_translational_distance; + } + + MotionModel getMotionModel() const + { + return motion_model_; + } + + void setMotionModel(MotionModel model) + { + motion_model_ = model; + } + /// Make the yaw component of a state's value vector be in the range [-Pi, Pi]. enforceBounds() also calls this /// function; /// Return true if a change is actually made @@ -82,6 +109,24 @@ class PlanarJointModel : public JointModel private: double angular_distance_weight_; + MotionModel motion_model_; + /// Only used for the differential drive motion model @see computeTurnDriveTurnGeometry + double min_translational_distance_; }; +/** + * @brief Compute the geometry to turn toward the target point, drive straight and then turn to target orientation + * @param[in] from A vector representing the initial position [x0, y0, theta0] + * @param[in] to A vector representing the target position [x1, y1, theta1] + * @param[in] min_translational_distance If the translational distance between \p from and \p to is less than this + * value the motion will be pure rotation (meters) + * @param[out] dx x1 - x0 (meters) + * @param[out] dy y1 - y0 (meters) + * @param[out] initial_turn The initial turn in radians to face the target + * @param[out] drive_angle The orientation in radians that the robot will be driving straight at + * @param[out] final_turn The final turn in radians to the target orientation + */ +void computeTurnDriveTurnGeometry(const double* from, const double* to, const double min_translational_distance, + double& dx, double& dy, double& initial_turn, double& drive_angle, + double& final_turn); } // namespace core } // namespace moveit diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp index 23ef7afed8..f52d5c5269 100644 --- a/moveit_core/robot_model/src/planar_joint_model.cpp +++ b/moveit_core/robot_model/src/planar_joint_model.cpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -45,7 +46,8 @@ namespace moveit { namespace core { -PlanarJointModel::PlanarJointModel(const std::string& name) : JointModel(name), angular_distance_weight_(1.0) +PlanarJointModel::PlanarJointModel(const std::string& name) + : JointModel(name), angular_distance_weight_(1.0), motion_model_(HOLONOMIC), min_translational_distance_(1e-5) { type_ = PLANAR; @@ -139,39 +141,127 @@ void PlanarJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNu normalizeRotation(values); } +void computeTurnDriveTurnGeometry(const double* from, const double* to, const double min_translational_distance, + double& dx, double& dy, double& initial_turn, double& drive_angle, double& final_turn) +{ + dx = to[0] - from[0]; + dy = to[1] - from[1]; + // If the translational distance between from & to states is very small, it will cause an unnecessary rotation since + // the robot will try to do the following rather than rotating directly to the orientation of `to` state + // 1- Align itself with the line connecting the origin of both states + // 2- Move to the origin of `to` state + // 3- Rotate so it have the same orientation as `to` state + // Example: from=[0.0, 0.0, 0.0] - to=[1e-31, 1e-31, -130°] + // here the robot will: rotate 45° -> move to the origin of `to` state -> rotate -175°, rather than rotating directly + // to -130° + // to fix this we added a joint property (default value is 1e-5) and make the movement pure rotation if the + // translational distance is less than this number + const double angle_straight_diff = std::hypot(dx, dy) > min_translational_distance ? + angles::shortest_angular_distance(from[2], std::atan2(dy, dx)) : + 0.0; + const double angle_backward_diff = + angles::normalize_angle(angle_straight_diff - boost::math::constants::pi()); + const double move_straight_cost = + std::abs(angle_straight_diff) + std::abs(angles::shortest_angular_distance(from[2] + angle_straight_diff, to[2])); + const double move_backward_cost = + std::abs(angle_backward_diff) + std::abs(angles::shortest_angular_distance(from[2] + angle_backward_diff, to[2])); + if (move_straight_cost <= move_backward_cost) + { + initial_turn = angle_straight_diff; + } + else + { + initial_turn = angle_backward_diff; + } + drive_angle = from[2] + initial_turn; + final_turn = angles::shortest_angular_distance(drive_angle, to[2]); +} + void PlanarJointModel::interpolate(const double* from, const double* to, const double t, double* state) const { - // interpolate position - state[0] = from[0] + (to[0] - from[0]) * t; - state[1] = from[1] + (to[1] - from[1]) * t; + if (motion_model_ == HOLONOMIC) + { + // interpolate position + state[0] = from[0] + (to[0] - from[0]) * t; + state[1] = from[1] + (to[1] - from[1]) * t; - // interpolate angle - double diff = to[2] - from[2]; - if (fabs(diff) <= boost::math::constants::pi()) - state[2] = from[2] + diff * t; - else + // interpolate angle + double diff = to[2] - from[2]; + if (fabs(diff) <= boost::math::constants::pi()) + state[2] = from[2] + diff * t; + else + { + if (diff > 0.0) + diff = 2.0 * boost::math::constants::pi() - diff; + else + diff = -2.0 * boost::math::constants::pi() - diff; + state[2] = from[2] - diff * t; + // input states are within bounds, so the following check is sufficient + if (state[2] > boost::math::constants::pi()) + state[2] -= 2.0 * boost::math::constants::pi(); + else if (state[2] < -boost::math::constants::pi()) + state[2] += 2.0 * boost::math::constants::pi(); + } + } + else if (motion_model_ == DIFF_DRIVE) { - if (diff > 0.0) - diff = 2.0 * boost::math::constants::pi() - diff; + double dx, dy, initial_turn, drive_angle, final_turn; + computeTurnDriveTurnGeometry(from, to, min_translational_distance_, dx, dy, initial_turn, drive_angle, final_turn); + + double initial_d = fabs(initial_turn) * angular_distance_weight_; + double drive_d = hypot(dx, dy); + double final_d = fabs(final_turn) * angular_distance_weight_; + + double total_d = initial_d + drive_d + final_d; + + double initial_frac = initial_d / total_d; + double drive_frac = drive_d / total_d; + double final_frac = final_d / total_d; + + double percent; + if (t <= initial_frac) + { + percent = t / initial_frac; + state[0] = from[0]; + state[1] = from[1]; + state[2] = from[2] + initial_turn * percent; + } + else if (t <= initial_frac + drive_frac) + { + percent = (t - initial_frac) / drive_frac; + state[0] = from[0] + dx * percent; + state[1] = from[1] + dy * percent; + state[2] = drive_angle; + } else - diff = -2.0 * boost::math::constants::pi() - diff; - state[2] = from[2] - diff * t; - // input states are within bounds, so the following check is sufficient - if (state[2] > boost::math::constants::pi()) - state[2] -= 2.0 * boost::math::constants::pi(); - else if (state[2] < -boost::math::constants::pi()) - state[2] += 2.0 * boost::math::constants::pi(); + { + percent = (t - initial_frac - drive_frac) / final_frac; + state[0] = to[0]; + state[1] = to[1]; + state[2] = drive_angle + final_turn * percent; + } } } double PlanarJointModel::distance(const double* values1, const double* values2) const { - double dx = values1[0] - values2[0]; - double dy = values1[1] - values2[1]; + if (motion_model_ == HOLONOMIC) + { + double dx = values1[0] - values2[0]; + double dy = values1[1] - values2[1]; - double d = fabs(values1[2] - values2[2]); - d = (d > boost::math::constants::pi()) ? 2.0 * boost::math::constants::pi() - d : d; - return sqrt(dx * dx + dy * dy) + angular_distance_weight_ * d; + double d = fabs(values1[2] - values2[2]); + d = (d > boost::math::constants::pi()) ? 2.0 * boost::math::constants::pi() - d : d; + return sqrt(dx * dx + dy * dy) + angular_distance_weight_ * d; + } + else if (motion_model_ == DIFF_DRIVE) + { + double dx, dy, initial_turn, drive_angle, final_turn; + computeTurnDriveTurnGeometry(values1, values2, min_translational_distance_, dx, dy, initial_turn, drive_angle, + final_turn); + return hypot(dx, dy) + angular_distance_weight_ * (fabs(initial_turn) + fabs(final_turn)); + } + return 0.0; } bool PlanarJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 16d0ce7e65..c0daa3ccef 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -976,6 +976,112 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const break; } } + for (const srdf::Model::JointProperty& property : srdf_model.getJointProperties(new_joint_model->getName())) + { + if (property.property_name_ == "angular_distance_weight") + { + double angular_distance_weight; + try + { + std::string::size_type sz; + angular_distance_weight = std::stod(property.value_, &sz); + if (sz != property.value_.size()) + { + ROS_WARN_STREAM_NAMED(LOGNAME, "Extra characters after property " << property.property_name_ << " for joint " + << property.joint_name_ << " as double: '" + << property.value_.substr(sz) << "'"); + } + } + catch (const std::invalid_argument& e) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Unable to parse property " << property.property_name_ << " for joint " + << property.joint_name_ << " as double: '" + << property.value_ << "'"); + continue; + } + + if (new_joint_model->getType() == JointModel::JointType::PLANAR) + { + ((PlanarJointModel*)new_joint_model)->setAngularDistanceWeight(angular_distance_weight); + } + else if (new_joint_model->getType() == JointModel::JointType::FLOATING) + { + ((FloatingJointModel*)new_joint_model)->setAngularDistanceWeight(angular_distance_weight); + } + else + { + ROS_ERROR_NAMED(LOGNAME, "Cannot apply property %s to joint type: %s", + property.property_name_.c_str(), + new_joint_model->getTypeName().c_str()); + } + } + else if (property.property_name_ == "motion_model") + { + if (new_joint_model->getType() != JointModel::JointType::PLANAR) + { + ROS_ERROR_NAMED(LOGNAME, "Cannot apply property %s to joint type: %s", + property.property_name_.c_str(), + new_joint_model->getTypeName().c_str()); + continue; + } + + PlanarJointModel::MotionModel motion_model; + if (property.value_ == "holonomic") + { + motion_model = PlanarJointModel::MotionModel::HOLONOMIC; + } + else if (property.value_ == "diff_drive") + { + motion_model = PlanarJointModel::MotionModel::DIFF_DRIVE; + } + else + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Unknown value for property " << property.property_name_ << " (" + << property.joint_name_ << "): '" << property.value_ + << "'"); + ROS_ERROR_NAMED(LOGNAME, "Valid values are 'holonomic' and 'diff_drive'"); + continue; + } + + ((PlanarJointModel*)new_joint_model)->setMotionModel(motion_model); + } + else if (property.property_name_ == "min_translational_distance") + { + if (new_joint_model->getType() != JointModel::JointType::PLANAR) + { + ROS_ERROR_NAMED(LOGNAME, "Cannot apply property %s to joint type: %s", + property.property_name_.c_str(), + new_joint_model->getTypeName().c_str()); + continue; + } + double min_translational_distance; + try + { + std::string::size_type sz; + min_translational_distance = std::stod(property.value_, &sz); + if (sz != property.value_.size()) + { + ROS_WARN_STREAM_NAMED(LOGNAME, "Extra characters after property " << property.property_name_ << " for joint " + << property.joint_name_ << " as double: '" + << property.value_.substr(sz) << "'"); + } + } + catch (const std::invalid_argument& e) + { + ROS_ERROR_NAMED(LOGNAME, "Unable to parse property %s for joint %s as double: '%s'", + property.property_name_.c_str(), + property.joint_name_.c_str(), + property.value_.c_str()); + continue; + } + + ((PlanarJointModel*)new_joint_model)->setMinTranslationalDistance(min_translational_distance); + } + else + { + ROS_ERROR_NAMED(LOGNAME, "Unknown joint property: %s", property.property_name_.c_str()); + } + } } return new_joint_model; diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h index 2ed487bd6c..3ad1b40f82 100644 --- a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h +++ b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h @@ -185,6 +185,13 @@ class RobotModelBuilder RobotModelBuilder& addEndEffector(const std::string& name, const std::string& parent_link, const std::string& parent_group = "", const std::string& component_group = ""); + /** \brief Adds a new joint property + * \param[in] joint_name The name of the joint the property is specified for + * \param[in] property_name The joint property name + * \param[in] value The value of the joint property + */ + void addJointProperty(const std::string& joint_name, const std::string& property_name, const std::string& value); + /** \} */ /** \brief Returns true if the building process so far has been valid. */ diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index b671692b6e..6b73d5d446 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -394,6 +394,12 @@ RobotModelBuilder& RobotModelBuilder::addEndEffector(const std::string& name, co return *this; } +void RobotModelBuilder::addJointProperty(const std::string& joint_name, const std::string& property_name, + const std::string& value) +{ + srdf_writer_->joint_properties_[joint_name].push_back({ joint_name, property_name, value }); +} + bool RobotModelBuilder::isValid() { return is_valid_; diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp index 879c7330d3..85f83ddd41 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp @@ -184,6 +184,34 @@ TEST_F(LoadPlanningModelsPr2, StateSpaceCopy) joint_model_state_space.freeState(state); } +// Run the OMPL sanity checks on the diff drive model +TEST(TestDiffDrive, TestStateSpace) +{ + moveit::core::RobotModelBuilder builder("mobile_base", "base_link"); + builder.addVirtualJoint("odom_combined", "base_link", "planar", "base_joint"); + builder.addJointProperty("base_joint", "motion_model", "diff_drive"); + builder.addGroup({}, { "base_joint" }, "base"); + ASSERT_TRUE(builder.isValid()); + + auto robot_model = builder.build(); + ompl_interface::ModelBasedStateSpaceSpecification spec(robot_model, "base"); + ompl_interface::JointModelStateSpace ss(spec); + ss.setPlanningVolume(-2, 2, -2, 2, -2, 2); + ss.setup(); + + bool passed = false; + try + { + ss.sanityChecks(); + passed = true; + } + catch (ompl::Exception& ex) + { + ROS_ERROR("Sanity checks did not pass: %s", ex.what()); + } + EXPECT_TRUE(passed); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv);