From 8023671fd4bf3fa2ae868b7eb848040b08128ca6 Mon Sep 17 00:00:00 2001 From: Joe Schornak Date: Thu, 27 Apr 2023 15:50:42 +0200 Subject: [PATCH] REMOVE WHEN #1986 merged: Fix collision checking in VisibilityConstraint commit 03dcec06b2ecbb16182c3c4519268b7cd0b8d8c3 Author: Joe Schornak Date: Fri Mar 10 19:16:19 2023 -0500 use same transform math for fixed and mobile frames commit e7b578d82a7237a0e05672edcb07d6989de15962 Author: Joe Schornak Date: Fri Mar 10 17:53:59 2023 -0500 reduce redundant transform calculations commit 3e1896ea557be6feab5d9ae7c9a27c9104b789f9 Author: Joe Schornak Date: Thu Mar 2 20:07:37 2023 -0500 clang-format fixes commit 2613f9cd1033b4fad71f8d5156508adb303d299f Author: Joe Schornak Date: Thu Mar 2 20:05:36 2023 -0500 delete commented-out code commit 648598d72036b0cdc477193ad692e0d670730e5a Author: Joe Schornak Date: Thu Mar 2 16:50:32 2023 -0500 disable visibility cone check if target radius is zero commit 718631f98f12052a22a9a8cc1fc842ef99dff911 Author: Joe Schornak Date: Wed Mar 1 20:10:35 2023 -0500 use local collision environment to check constraint --- .../kinematic_constraint.h | 35 +-- .../src/kinematic_constraint.cpp | 225 +++++++++--------- 2 files changed, 130 insertions(+), 130 deletions(-) diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index 9edc00fc43d..0d5d2e96747 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -774,9 +774,13 @@ class VisibilityConstraint : public KinematicConstraint * \brief Configure the constraint based on a * moveit_msgs::msg::VisibilityConstraint * - * For the configure command to be successful, the target radius - * must be non-zero (negative values will have the absolute value - * taken). If cone sides are less than 3, a value of 3 will be used. + * For the configure command to be successful, one of the three possible + * constraint criteria must be set to a non-zero value: + * - The target radius (negative values will have the absolute value taken). + * - The range angle. + * - The view angle. + * + * If cone sides are less than 3, a value of 3 will be used. * * @param [in] vc moveit_msgs::msg::VisibilityConstraint for configuration * @@ -807,11 +811,13 @@ class VisibilityConstraint : public KinematicConstraint /** * \brief Gets a trimesh shape representing the visibility cone * - * @param [in] state The state from which to produce the cone + * @param [in] tform_world_to_sensor Transform from the world to the sensor frame + * @param [in] tform_world_to_target Transform from the world to the target frame * * @return The shape associated with the cone */ - shapes::Mesh* getVisibilityCone(const moveit::core::RobotState& state) const; + shapes::Mesh* getVisibilityCone(const Eigen::Isometry3d& tform_world_to_sensor, + const Eigen::Isometry3d& tform_world_to_target) const; /** * \brief Adds markers associated with the visibility cone, sensor @@ -842,16 +848,15 @@ class VisibilityConstraint : public KinematicConstraint */ bool decideContact(const collision_detection::Contact& contact) const; - collision_detection::CollisionEnvPtr collision_env_; /**< \brief A copy of the collision robot maintained for - collision checking the cone against robot links */ - bool mobile_sensor_frame_; /**< \brief True if the sensor is a non-fixed frame relative to the transform frame */ - bool mobile_target_frame_; /**< \brief True if the target is a non-fixed frame relative to the transform frame */ - std::string target_frame_id_; /**< \brief The target frame id */ - std::string sensor_frame_id_; /**< \brief The sensor frame id */ - Eigen::Isometry3d sensor_pose_; /**< \brief The sensor pose transformed into the transform frame */ - int sensor_view_direction_; /**< \brief Storage for the sensor view direction */ - Eigen::Isometry3d target_pose_; /**< \brief The target pose transformed into the transform frame */ - unsigned int cone_sides_; /**< \brief Storage for the cone sides */ + moveit::core::RobotModelConstPtr robot_model_; /**< \brief A copy of the robot model used to create collision + environments to check the cone against robot links */ + + std::string target_frame_id_; /**< \brief The target frame id */ + std::string sensor_frame_id_; /**< \brief The sensor frame id */ + Eigen::Isometry3d sensor_pose_; /**< \brief The sensor pose transformed into the transform frame */ + int sensor_view_direction_; /**< \brief Storage for the sensor view direction */ + Eigen::Isometry3d target_pose_; /**< \brief The target pose transformed into the transform frame */ + unsigned int cone_sides_; /**< \brief Storage for the cone sides */ EigenSTL::vector_Vector3d points_; /**< \brief A set of points along the base of the circle */ double target_radius_; /**< \brief Storage for the target radius */ double max_view_angle_; /**< \brief Storage for the max view angle */ diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index d1878d1669a..9d3fa2503c0 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -786,15 +786,13 @@ void OrientationConstraint::print(std::ostream& out) const } VisibilityConstraint::VisibilityConstraint(const moveit::core::RobotModelConstPtr& model) - : KinematicConstraint(model), collision_env_(std::make_shared(model)) + : KinematicConstraint(model), robot_model_{ model } { type_ = VISIBILITY_CONSTRAINT; } void VisibilityConstraint::clear() { - mobile_sensor_frame_ = false; - mobile_target_frame_ = false; target_frame_id_ = ""; sensor_frame_id_ = ""; sensor_pose_ = Eigen::Isometry3d::Identity(); @@ -845,15 +843,10 @@ bool VisibilityConstraint::configure(const moveit_msgs::msg::VisibilityConstrain { tf.transformPose(vc.target_pose.header.frame_id, target_pose_, target_pose_); target_frame_id_ = tf.getTargetFrame(); - mobile_target_frame_ = false; - // transform won't change, so apply it now - for (Eigen::Vector3d& point : points_) - point = target_pose_ * point; } else { target_frame_id_ = vc.target_pose.header.frame_id; - mobile_target_frame_ = true; } tf2::fromMsg(vc.sensor_pose.pose, sensor_pose_); @@ -863,12 +856,10 @@ bool VisibilityConstraint::configure(const moveit_msgs::msg::VisibilityConstrain { tf.transformPose(vc.sensor_pose.header.frame_id, sensor_pose_, sensor_pose_); sensor_frame_id_ = tf.getTargetFrame(); - mobile_sensor_frame_ = false; } else { sensor_frame_id_ = vc.sensor_pose.header.frame_id; - mobile_sensor_frame_ = true; } if (vc.weight <= std::numeric_limits::epsilon()) @@ -883,7 +874,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::msg::VisibilityConstrain max_range_angle_ = vc.max_range_angle; sensor_view_direction_ = vc.sensor_view_direction; - return target_radius_ > std::numeric_limits::epsilon(); + return enabled(); } bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin) const @@ -917,28 +908,30 @@ bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin bool VisibilityConstraint::enabled() const { - return target_radius_ > std::numeric_limits::epsilon(); + return (target_radius_ > std::numeric_limits::epsilon()) || + (max_view_angle_ > std::numeric_limits::epsilon()) || + (max_range_angle_ > std::numeric_limits::epsilon()); } -shapes::Mesh* VisibilityConstraint::getVisibilityCone(const moveit::core::RobotState& state) const +shapes::Mesh* VisibilityConstraint::getVisibilityCone(const Eigen::Isometry3d& tform_world_to_sensor, + const Eigen::Isometry3d& tform_world_to_target) const { // the current pose of the sensor + const Eigen::Isometry3d& sp = tform_world_to_sensor; - const Eigen::Isometry3d& sp = - mobile_sensor_frame_ ? state.getFrameTransform(sensor_frame_id_) * sensor_pose_ : sensor_pose_; - const Eigen::Isometry3d& tp = - mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_; + // the current pose of the target + const Eigen::Isometry3d& tp = tform_world_to_target; // transform the points on the disc to the desired target frame const EigenSTL::vector_Vector3d* points = &points_; std::unique_ptr temp_points; - if (mobile_target_frame_) + + temp_points = std::make_unique(points_.size()); + for (std::size_t i = 0; i < points_.size(); ++i) { - temp_points = std::make_unique(points_.size()); - for (std::size_t i = 0; i < points_.size(); ++i) - temp_points->at(i) = tp * points_[i]; - points = temp_points.get(); + temp_points->at(i) = tp * points_[i]; } + points = temp_points.get(); // allocate memory for a mesh to represent the visibility cone shapes::Mesh* m = new shapes::Mesh(); @@ -997,7 +990,13 @@ shapes::Mesh* VisibilityConstraint::getVisibilityCone(const moveit::core::RobotS void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state, visualization_msgs::msg::MarkerArray& markers) const { - shapes::Mesh* m = getVisibilityCone(state); + // getFrameTransform() returns a valid isometry by contract + // sensor_pose_ is valid isometry (checked in configure()) + const Eigen::Isometry3d& sp = state.getFrameTransform(sensor_frame_id_) * sensor_pose_; + // target_pose_ is valid isometry (checked in configure()) + const Eigen::Isometry3d& tp = state.getFrameTransform(target_frame_id_) * target_pose_; + + shapes::Mesh* m = getVisibilityCone(sp, tp); visualization_msgs::msg::Marker mk; shapes::constructMarkerFromShape(m, mk); delete m; @@ -1023,14 +1022,6 @@ void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state, markers.markers.push_back(mk); - // getFrameTransform() returns a valid isometry by contract - // sensor_pose_ is valid isometry (checked in configure()) - const Eigen::Isometry3d& sp = - mobile_sensor_frame_ ? state.getFrameTransform(sensor_frame_id_) * sensor_pose_ : sensor_pose_; - // target_pose_ is valid isometry (checked in configure()) - const Eigen::Isometry3d& tp = - mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_; - visualization_msgs::msg::Marker mka; mka.type = visualization_msgs::msg::Marker::ARROW; mka.action = visualization_msgs::msg::Marker::ADD; @@ -1071,109 +1062,113 @@ void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state, ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::RobotState& state, bool verbose) const { - if (target_radius_ <= std::numeric_limits::epsilon()) - return ConstraintEvaluationResult(true, 0.0); - - if (max_view_angle_ > 0.0 || max_range_angle_ > 0.0) - { - // getFrameTransform() returns a valid isometry by contract - // sensor_pose_ is valid isometry (checked in configure()) - const Eigen::Isometry3d& sp = - mobile_sensor_frame_ ? state.getFrameTransform(sensor_frame_id_) * sensor_pose_ : sensor_pose_; - // target_pose_ is valid isometry (checked in configure()) - const Eigen::Isometry3d& tp = - mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_; + // getFrameTransform() returns a valid isometry by contract + // sensor_pose_ is valid isometry (checked in configure()) + const Eigen::Isometry3d& tform_world_to_sensor = state.getFrameTransform(sensor_frame_id_) * sensor_pose_; + // target_pose_ is valid isometry (checked in configure()) + const Eigen::Isometry3d& tform_world_to_target = state.getFrameTransform(target_frame_id_) * target_pose_; - // necessary to do subtraction as SENSOR_Z is 0 and SENSOR_X is 2 - const Eigen::Vector3d& normal2 = sp.linear().col(2 - sensor_view_direction_); + // necessary to do subtraction as SENSOR_Z is 0 and SENSOR_X is 2 + const Eigen::Vector3d& sensor_view_axis = tform_world_to_sensor.linear().col(2 - sensor_view_direction_); - if (max_view_angle_ > 0.0) + // Check view angle constraint + if (max_view_angle_ > std::numeric_limits::epsilon()) + { + const Eigen::Vector3d& normal1 = tform_world_to_target.linear().col(2) * -1.0; // along Z axis and inverted + double dp = sensor_view_axis.dot(normal1); + double ang = acos(dp); + if (dp < 0.0) { - const Eigen::Vector3d& normal1 = tp.linear().col(2) * -1.0; // along Z axis and inverted - double dp = normal2.dot(normal1); - double ang = acos(dp); - if (dp < 0.0) - { - if (verbose) - { - RCLCPP_INFO(LOGGER, "Visibility constraint is violated because the sensor is looking at " - "the wrong side"); - } - return ConstraintEvaluationResult(false, 0.0); - } - if (max_view_angle_ < ang) + if (verbose) { - if (verbose) - { - RCLCPP_INFO(LOGGER, - "Visibility constraint is violated because the view angle is %lf " - "(above the maximum allowed of %lf)", - ang, max_view_angle_); - } - return ConstraintEvaluationResult(false, 0.0); + RCLCPP_INFO(LOGGER, "Visibility constraint is violated because the sensor is looking at " + "the wrong side"); } + return ConstraintEvaluationResult(false, 0.0); } - if (max_range_angle_ > 0.0) + if (max_view_angle_ < ang) { - const Eigen::Vector3d& dir = (tp.translation() - sp.translation()).normalized(); - double dp = normal2.dot(dir); - if (dp < 0.0) + if (verbose) { - if (verbose) - { - RCLCPP_INFO(LOGGER, "Visibility constraint is violated because the sensor is looking at " - "the wrong side"); - } - return ConstraintEvaluationResult(false, 0.0); + RCLCPP_INFO(LOGGER, + "Visibility constraint is violated because the view angle is %lf " + "(above the maximum allowed of %lf)", + ang, max_view_angle_); } + return ConstraintEvaluationResult(false, 0.0); + } + } - double ang = acos(dp); - if (max_range_angle_ < ang) - { - if (verbose) - { - RCLCPP_INFO(LOGGER, - "Visibility constraint is violated because the range angle is %lf " - "(above the maximum allowed of %lf)", - ang, max_range_angle_); - } - return ConstraintEvaluationResult(false, 0.0); - } + // Check range angle constraint + if (max_range_angle_ > std::numeric_limits::epsilon()) + { + const Eigen::Vector3d& dir = + (tform_world_to_target.translation() - tform_world_to_sensor.translation()).normalized(); + double dp = sensor_view_axis.dot(dir); + if (dp < 0.0) + { + if (verbose) + RCLCPP_INFO(LOGGER, "Visibility constraint is violated because the sensor is looking at " + "the wrong side"); + return ConstraintEvaluationResult(false, 0.0); + } + + double ang = acos(dp); + if (max_range_angle_ < ang) + { + if (verbose) + RCLCPP_INFO(LOGGER, + "Visibility constraint is violated because the range angle is %lf " + "(above the maximum allowed of %lf)", + ang, max_range_angle_); + return ConstraintEvaluationResult(false, 0.0); } } - shapes::Mesh* m = getVisibilityCone(state); - if (!m) - return ConstraintEvaluationResult(false, 0.0); + // Check visibility cone collision constraint + if (target_radius_ > std::numeric_limits::epsilon()) + { + shapes::Mesh* m = getVisibilityCone(tform_world_to_sensor, tform_world_to_target); + if (!m) + { + RCLCPP_ERROR(LOGGER, "Visibility constraint is violated because we could not create the visibility cone mesh."); + return ConstraintEvaluationResult(false, 0.0); + } - // add the visibility cone as an object - collision_env_->getWorld()->addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Isometry3d::Identity()); + // add the visibility cone as an object + const auto collision_env_local = std::make_shared(robot_model_); + collision_env_local->getWorld()->addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Isometry3d::Identity()); - // check for collisions between the robot and the cone - collision_detection::CollisionRequest req; - collision_detection::CollisionResult res; - collision_detection::AllowedCollisionMatrix acm; - collision_detection::DecideContactFn fn = [this](collision_detection::Contact& contact) { - return decideContact(contact); - }; - acm.setDefaultEntry(std::string("cone"), fn); + // check for collisions between the robot and the cone + collision_detection::AllowedCollisionMatrix acm; + collision_detection::DecideContactFn fn = [this](collision_detection::Contact& contact) { + return decideContact(contact); + }; + acm.setDefaultEntry(std::string("cone"), fn); - req.contacts = true; - req.verbose = verbose; - req.max_contacts = 1; - collision_env_->checkRobotCollision(req, res, state, acm); + collision_detection::CollisionRequest req; + req.contacts = true; + req.verbose = verbose; + req.max_contacts = 1; - if (verbose) - { - std::stringstream ss; - m->print(ss); - RCLCPP_INFO(LOGGER, "Visibility constraint %ssatisfied. Visibility cone approximation:\n %s", - res.collision ? "not " : "", ss.str().c_str()); - } + collision_detection::CollisionResult res; + collision_env_local->checkRobotCollision(req, res, state, acm); - collision_env_->getWorld()->removeObject("cone"); + if (verbose) + { + std::stringstream ss; + m->print(ss); + RCLCPP_INFO(LOGGER, "Visibility constraint %ssatisfied. Visibility cone approximation:\n %s", + res.collision ? "not " : "", ss.str().c_str()); + } + + collision_env_local->getWorld()->removeObject("cone"); + + return ConstraintEvaluationResult(!res.collision, res.collision ? res.contacts.begin()->second.front().depth : 0.0); + } - return ConstraintEvaluationResult(!res.collision, res.collision ? res.contacts.begin()->second.front().depth : 0.0); + // Constraint evaluation succeeded if we made it here + return ConstraintEvaluationResult(true, 0.0); } bool VisibilityConstraint::decideContact(const collision_detection::Contact& contact) const