kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::OrientationConstraint::decide(const planning_models::KinematicState &state, bool verbose) const
{
if (!link_model_)
return ConstraintEvaluationResult(true, 0.0);
const planning_models::KinematicState::LinkState *link_state = state.getLinkState(link_model_->getName());
if (!link_state)
{
ROS_WARN_STREAM("No link in state with name '" << link_model_->getName() << "'");
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); // XYZ
}
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_ && xyz(1) < absolute_y_axis_tolerance_ && xyz(0) < absolute_x_axis_tolerance_;
if (verbose)
{
Eigen::Quaterniond q_act(link_state->getGlobalLinkTransform().rotation());
Eigen::Quaterniond q_des(desired_rotation_matrix_);
ROS_INFO("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 planning_models::KinematicState &state, bool verbose) const
{
if (!link_model_ || constraint_region_.empty())
return ConstraintEvaluationResult(true, 0.0);
const planning_models::KinematicState::LinkState *link_state = state.getLinkState(link_model_->getName());
if (!link_state)
{
ROS_WARN_STREAM("No link in state with name '" << link_model_->getName() << "'");
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);
if (result || (i + 1 == constraint_region_pose_.size()))
return 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);
if (result || (i + 1 == constraint_region_.size()))
return finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(), constraint_weight_, result, verbose);
}
}
return ConstraintEvaluationResult(false, 0.0);
}
bool world_transform(std::string frame_id,
const planning_models::KinematicState &state,
btTransform &transform) {
if (!frame_id.compare(state.getKinematicModel()->getRoot()->getParentFrameId())) {
//identity transform
transform.setIdentity();
return true;
}
if (!frame_id.compare(state.getKinematicModel()->getRoot()->getChildFrameId())) {
transform = state.getRootTransform();
return true;
}
const planning_models::KinematicState::LinkState *link =
state.getLinkState(frame_id);
if (!link) {
ROS_ERROR("Unable to find link %s in kinematic state", frame_id.c_str());
return false;
}
transform = link->getGlobalLinkTransform();
return true;
}
bool constraint_samplers::IKConstraintSampler::samplePose(Eigen::Vector3d &pos, Eigen::Quaterniond &quat,
const planning_models::KinematicState &ks,
unsigned int max_attempts)
{
if (sampling_pose_.position_constraint_)
{
const std::vector<bodies::BodyPtr> &b = sampling_pose_.position_constraint_->getConstraintRegions();
if (!b.empty())
{
bool found = false;
std::size_t k = random_number_generator_.uniformInteger(0, b.size() - 1);
for (std::size_t i = 0 ; i < b.size() ; ++i)
if (b[(i+k) % b.size()]->samplePointInside(random_number_generator_, max_attempts, pos))
{
found = true;
break;
}
if (!found)
{
ROS_ERROR("Unable to sample a point inside the constraint region");
return false;
}
}
else
{
ROS_ERROR("Unable to sample a point inside the constraint region. Constraint region is empty when it should not be.");
return false;
}
// if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the model
if (sampling_pose_.position_constraint_->mobileReferenceFrame())
{
const planning_models::KinematicState::LinkState *ls = ks.getLinkState(sampling_pose_.position_constraint_->getReferenceFrame());
pos = ls->getGlobalLinkTransform() * pos;
}
}
else
{
// do FK for rand state
planning_models::KinematicState tempState(ks);
planning_models::KinematicState::JointStateGroup *tmp = tempState.getJointStateGroup(jmg_->getName());
if (tmp)
{
tmp->setToRandomValues();
pos = tempState.getLinkState(sampling_pose_.orientation_constraint_->getLinkModel()->getName())->getGlobalLinkTransform().translation();
}
else
{
ROS_ERROR("Passed a JointStateGroup for a mismatching JointModelGroup");
return false;
}
}
if (sampling_pose_.orientation_constraint_)
{
// sample a rotation matrix within the allowed bounds
double angle_x = 2.0 * (random_number_generator_.uniform01() - 0.5) * sampling_pose_.orientation_constraint_->getXAxisTolerance();
double angle_y = 2.0 * (random_number_generator_.uniform01() - 0.5) * sampling_pose_.orientation_constraint_->getYAxisTolerance();
double angle_z = 2.0 * (random_number_generator_.uniform01() - 0.5) * sampling_pose_.orientation_constraint_->getZAxisTolerance();
Eigen::Affine3d diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX())
* Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));
Eigen::Affine3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.rotation());
quat = Eigen::Quaterniond(reqr.rotation());
// if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the model
if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
{
const planning_models::KinematicState::LinkState *ls = ks.getLinkState(sampling_pose_.orientation_constraint_->getReferenceFrame());
Eigen::Affine3d rt(ls->getGlobalLinkTransform().rotation() * quat.toRotationMatrix());
quat = Eigen::Quaterniond(rt.rotation());
}
}
else
{
// sample a random orientation
double q[4];
random_number_generator_.quaternion(q);
quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]);
}
// if there is an offset, we need to undo the induced rotation in the sampled transform origin (point)
if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->hasLinkOffset())
// the rotation matrix that corresponds to the desired orientation
pos = pos - quat.toRotationMatrix() * sampling_pose_.position_constraint_->getLinkOffset();
// we now have the transform we wish to perform IK for, in the planning frame
if (transform_ik_)
{
// we need to convert this transform to the frame expected by the IK solver
// both the planning frame and the frame for the IK are assumed to be robot links
Eigen::Affine3d ikq(Eigen::Translation3d(pos) * quat.toRotationMatrix());
const planning_models::KinematicState::LinkState *ls = ks.getLinkState(ik_frame_);
ikq = ls->getGlobalLinkTransform().inverse() * ikq;
pos = ikq.translation();
quat = Eigen::Quaterniond(ikq.rotation());
}
return true;
}