kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::OrientationConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
if (!link_model_)
return ConstraintEvaluationResult(true, 0.0);
const robot_state::LinkState *link_state = state.getLinkState(link_model_->getName());
if (!link_state)
{
logWarn("No link in state with name '%s'", link_model_->getName().c_str());
return ConstraintEvaluationResult(false, 0.0);
}
Eigen::Vector3d xyz;
if (mobile_frame_)
{
Eigen::Matrix3d tmp;
tf_->transformRotationMatrix(state, desired_rotation_frame_id_, desired_rotation_matrix_, tmp);
Eigen::Affine3d diff(tmp.inverse() * link_state->getGlobalLinkTransform().rotation());
xyz = diff.rotation().eulerAngles(0, 1, 2);
// 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
}
else
{
Eigen::Affine3d diff(desired_rotation_matrix_inv_ * link_state->getGlobalLinkTransform().rotation());
xyz = diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
}
xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
bool result = xyz(2) < absolute_z_axis_tolerance_+std::numeric_limits<double>::epsilon()
&& xyz(1) < absolute_y_axis_tolerance_+std::numeric_limits<double>::epsilon()
&& xyz(0) < absolute_x_axis_tolerance_+std::numeric_limits<double>::epsilon();
if (verbose)
{
Eigen::Quaterniond q_act(link_state->getGlobalLinkTransform().rotation());
Eigen::Quaterniond q_des(desired_rotation_matrix_);
logInform("Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f",
result ? "satisfied" : "violated", link_model_->getName().c_str(),
q_des.x(), q_des.y(), q_des.z(), q_des.w(),
q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2),
absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_);
}
return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2)));
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::PositionConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
if (!link_model_ || constraint_region_.empty())
return ConstraintEvaluationResult(true, 0.0);
const robot_state::LinkState *link_state = state.getLinkState(link_model_->getName());
if (!link_state)
{
logWarn("No link in state with name '%s'", link_model_->getName().c_str());
return ConstraintEvaluationResult(false, 0.0);
}
Eigen::Vector3d pt = link_state->getGlobalLinkTransform() * offset_;
if (mobile_frame_)
{
Eigen::Affine3d tmp;
for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
{
tf_->transformPose(state, constraint_frame_id_, constraint_region_pose_[i], tmp);
bool result = constraint_region_[i]->cloneAt(tmp)->containsPoint(pt, verbose);
if (result || (i + 1 == constraint_region_pose_.size()))
return finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_, result, verbose);
else
finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_, result, verbose);
}
}
else
{
for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
{
bool result = constraint_region_[i]->containsPoint(pt, true);
if (result || (i + 1 == constraint_region_.size()))
return finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(), constraint_weight_, result, verbose);
else
finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(), constraint_weight_, result, verbose);
}
}
return ConstraintEvaluationResult(false, 0.0);
}