bool kinematic_constraints::VisibilityConstraint::equal(const KinematicConstraint &other, double margin) const
{
if (other.getType() != type_)
return false;
const VisibilityConstraint &o = static_cast<const VisibilityConstraint&>(other);
if (target_frame_id_ == o.target_frame_id_ && sensor_frame_id_ == o.sensor_frame_id_ &&
cone_sides_ == o.cone_sides_ && sensor_view_direction_ == o.sensor_view_direction_)
{
if (fabs(max_view_angle_ - o.max_view_angle_) > margin ||
fabs(target_radius_ - o.target_radius_) > margin)
return false;
Eigen::Affine3d diff = sensor_pose_.inverse() * o.sensor_pose_;
if (diff.translation().norm() > margin)
return false;
if (!diff.rotation().isIdentity(margin))
return false;
diff = target_pose_.inverse() * o.target_pose_;
if (diff.translation().norm() > margin)
return false;
if (!diff.rotation().isIdentity(margin))
return false;
return true;
}
return false;
}
bool kinematic_constraints::PositionConstraint::equal(const KinematicConstraint &other, double margin) const
{
if (other.getType() != type_)
return false;
const PositionConstraint &o = static_cast<const PositionConstraint&>(other);
if (link_model_ == o.link_model_ && constraint_frame_id_ == o.constraint_frame_id_)
{
if ((offset_ - o.offset_).norm() > margin)
return false;
if (constraint_region_.size() != o.constraint_region_.size())
return false;
for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
{
Eigen::Affine3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[i];
if (diff.translation().norm() > margin)
return false;
if (!diff.rotation().isIdentity(margin))
return false;
if (fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[i]->computeVolume()) >= margin)
return false;
}
return true;
}
return false;
}
bool kinematic_constraints::JointConstraint::equal(const KinematicConstraint &other, double margin) const
{
if (other.getType() != type_)
return false;
const JointConstraint &o = static_cast<const JointConstraint&>(other);
if (o.joint_model_ == joint_model_ && o.local_variable_name_ == local_variable_name_)
return fabs(joint_position_ - o.joint_position_) <= margin &&
fabs(joint_tolerance_above_ - o.joint_tolerance_above_) <= margin &&
fabs(joint_tolerance_below_ - o.joint_tolerance_below_) <= margin;
return false;
}
bool kinematic_constraints::OrientationConstraint::equal(const KinematicConstraint &other, double margin) const
{
if (other.getType() != type_)
return false;
const OrientationConstraint &o = static_cast<const OrientationConstraint&>(other);
if (o.link_model_ == link_model_ && desired_rotation_frame_id_ == o.desired_rotation_frame_id_)
{
Eigen::Matrix3d diff = desired_rotation_matrix_.inverse() * o.desired_rotation_matrix_;
if (!diff.isIdentity(margin))
return false;
return fabs(absolute_x_axis_tolerance_ - o.absolute_x_axis_tolerance_) <= margin &&
fabs(absolute_y_axis_tolerance_ - o.absolute_y_axis_tolerance_) <= margin &&
fabs(absolute_z_axis_tolerance_ - o.absolute_z_axis_tolerance_) <= margin;
}
return false;
}
bool kinematic_constraints::PositionConstraint::equal(const KinematicConstraint &other, double margin) const
{
if (other.getType() != type_)
return false;
const PositionConstraint &o = static_cast<const PositionConstraint&>(other);
if (link_model_ == o.link_model_ && constraint_frame_id_ == o.constraint_frame_id_)
{
if ((offset_ - o.offset_).norm() > margin)
return false;
std::vector<bool> other_region_matches_this(constraint_region_.size(), false);
for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
{
bool some_match = false;
//need to check against all other regions
for(std::size_t j = 0 ; j < o.constraint_region_.size() ; ++j)
{
Eigen::Affine3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[j];
if (diff.translation().norm() < margin
&& diff.rotation().isIdentity(margin)
&& constraint_region_[i]->getType() == o.constraint_region_[j]->getType()
&& fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[j]->computeVolume()) < margin)
{
some_match = true;
//can't break, as need to do matches the other way as well
other_region_matches_this[j] = true;
}
}
if (!some_match)
return false;
}
for (std::size_t i = 0 ; i < o.constraint_region_.size() ; ++i)
if (!other_region_matches_this[i])
return false;
return true;
}
return false;
}
