Skip to content

Commit

Permalink
Fix collision checking in VisibilityConstraint (#1986)
Browse files Browse the repository at this point in the history
  • Loading branch information
@schornakj
schornakj committed May 9, 2023
1 parent c85b6a3 commit 7c6da55
Show file tree
Hide file tree
Showing 2 changed files with 130 additions and 126 deletions.
35 changes: 20 additions & 15 deletions ...it_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
*
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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 */
Expand Down
221 changes: 110 additions & 111 deletions moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<collision_detection::CollisionEnvFCL>(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();
Expand Down Expand Up @@ -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_);
Expand All @@ -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<double>::epsilon())
Expand All @@ -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<double>::epsilon();
return enabled();
}

bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin) const
Expand Down Expand Up @@ -917,28 +908,30 @@ bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin

bool VisibilityConstraint::enabled() const
{
return target_radius_ > std::numeric_limits<double>::epsilon();
return (target_radius_ > std::numeric_limits<double>::epsilon()) ||
(max_view_angle_ > std::numeric_limits<double>::epsilon()) ||
(max_range_angle_ > std::numeric_limits<double>::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<EigenSTL::vector_Vector3d> temp_points;
if (mobile_target_frame_)

temp_points = std::make_unique<EigenSTL::vector_Vector3d>(points_.size());
for (std::size_t i = 0; i < points_.size(); ++i)
{
temp_points = std::make_unique<EigenSTL::vector_Vector3d>(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();
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -1071,109 +1062,117 @@ 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<double>::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<double>::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)
{
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 sensor is looking at "
"the wrong side");
}
if (max_view_angle_ < ang)
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 view angle is %lf "
"(above the maximum allowed of %lf)",
ang, max_view_angle_);
}
return ConstraintEvaluationResult(false, 0.0);
}
if (max_range_angle_ > 0.0)
}

// Check range angle constraint
if (max_range_angle_ > std::numeric_limits<double>::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)
{
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 sensor is looking at "
"the wrong side");
}
return ConstraintEvaluationResult(false, 0.0);
}

double ang = acos(dp);
if (max_range_angle_ < ang)
double ang = acos(dp);
if (max_range_angle_ < ang)
{
if (verbose)
{
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);
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<double>::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<collision_detection::CollisionEnvFCL>(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
Expand Down

0 comments on commit 7c6da55

Please sign in to comment.