kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide(const planning_models::KinematicState &state, bool verbose) const { if (!joint_model_) return ConstraintEvaluationResult(true, 0.0); const planning_models::KinematicState::JointState *joint = state.getJointState(joint_model_->getName()); if (!joint) { ROS_WARN_STREAM("No joint in state with name '" << joint_model_->getName() << "'"); return ConstraintEvaluationResult(true, 0.0); } double current_joint_position = joint->getVariableValues()[0]; if (!local_variable_name_.empty()) { const std::map<std::string, unsigned int> &index_map = joint->getVariableIndexMap(); std::map<std::string, unsigned int>::const_iterator it = index_map.find(joint_variable_name_); if (it == index_map.end()) { ROS_WARN_STREAM("Local name '" << local_variable_name_ << "' is not known to joint state with name '" << joint_model_->getName() << "'"); return ConstraintEvaluationResult(true, 0.0); } else current_joint_position = joint->getVariableValues()[it->second]; } double dif = 0.0; // compute signed shortest distance for continuous joints if (joint_is_continuous_) { dif = normalizeAngle(current_joint_position) - joint_position_; if (dif > boost::math::constants::pi<double>()) dif = 2.0*boost::math::constants::pi<double>() - dif; else if (dif < -boost::math::constants::pi<double>()) dif += 2.0*boost::math::constants::pi<double>(); // we include a sign change to have dif > 0 // however, we want to include proper sign for diff, as the tol below is may be different from tol above if (current_joint_position < joint_position_) dif = -dif; } else dif = current_joint_position - joint_position_; // check bounds bool result = dif <= joint_tolerance_above_ && dif >= -joint_tolerance_below_; if (verbose) ROS_INFO("Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, tolerance_above: %f, tolerance_below: %f", result ? "satisfied" : "violated", joint_variable_name_.c_str(), current_joint_position, joint_position_, joint_tolerance_above_, joint_tolerance_below_); return ConstraintEvaluationResult(result, constraint_weight_ * fabs(dif)); }
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide(const robot_state::RobotState &state, bool verbose) const { if (!joint_model_) return ConstraintEvaluationResult(true, 0.0); const robot_state::JointState *joint = state.getJointState(joint_model_->getName()); if (!joint) { logWarn("No joint in state with name '%s'", joint_model_->getName().c_str()); return ConstraintEvaluationResult(true, 0.0); } double current_joint_position = joint->getVariableValues()[0]; if (!local_variable_name_.empty()) { const std::map<std::string, unsigned int> &index_map = joint->getVariableIndexMap(); std::map<std::string, unsigned int>::const_iterator it = index_map.find(joint_variable_name_); if (it == index_map.end()) { logWarn("Local name '%s' is not known to joint state with name '%s'", local_variable_name_.c_str(), joint_model_->getName().c_str()); return ConstraintEvaluationResult(true, 0.0); } else current_joint_position = joint->getVariableValues()[it->second]; } double dif = 0.0; // compute signed shortest distance for continuous joints if (joint_is_continuous_) { dif = normalizeAngle(current_joint_position) - joint_position_; if (dif > boost::math::constants::pi<double>()) dif = 2.0*boost::math::constants::pi<double>() - dif; else if (dif < -boost::math::constants::pi<double>()) dif += 2.0*boost::math::constants::pi<double>(); // we include a sign change to have dif > 0 } else dif = current_joint_position - joint_position_; // check bounds bool result = dif <= (joint_tolerance_above_+2*std::numeric_limits<double>::epsilon()) && dif >= (-joint_tolerance_below_-2*std::numeric_limits<double>::epsilon()); if (verbose) logInform("Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, tolerance_above: %f, tolerance_below: %f", result ? "satisfied" : "violated", joint_variable_name_.c_str(), current_joint_position, joint_position_, joint_tolerance_above_, joint_tolerance_below_); return ConstraintEvaluationResult(result, constraint_weight_ * fabs(dif)); }
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))); }
// helper function to avoid code duplication static inline kinematic_constraints::ConstraintEvaluationResult finishPositionConstraintDecision(const Eigen::Vector3d &pt, const Eigen::Vector3d &desired, const std::string &name, double weight, bool result, bool verbose) { if (verbose) ROS_INFO("Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f", result ? "satisfied" : "violated", name.c_str(), desired.x(), desired.y(), desired.z(), pt.x(), pt.y(), pt.z()); double dx = desired.x() - pt.x(); double dy = desired.y() - pt.y(); double dz = desired.z() - pt.z(); return ConstraintEvaluationResult(result, weight * sqrt(dx * dx + dy * dy + dz * dz)); }
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); }
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::VisibilityConstraint::decide(const planning_models::KinematicState &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) { const Eigen::Affine3d &sp = mobile_sensor_frame_ ? tf_->getTransform(state, sensor_frame_id_) * sensor_pose_ : sensor_pose_; const Eigen::Affine3d &tp = mobile_target_frame_ ? tf_->getTransform(state, target_frame_id_) * target_pose_ : target_pose_; const Eigen::Vector3d &normal2 = sp.rotation().col(sensor_view_direction_); if (max_view_angle_ > 0.0) { const Eigen::Vector3d &normal1 = tp.rotation().col(2); // along Z axis double dp = normal2.dot(normal1); if (dp < 0.0) { if (verbose) ROS_INFO("Visibility constraint is violated because the sensor is looking at the wrong side"); return ConstraintEvaluationResult(false, 0.0); } double ang = acos(dp); if (max_view_angle_ < ang) { if (verbose) ROS_INFO("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) { const Eigen::Vector3d &dir = (tp.translation() - sp.translation()).normalized(); double dp = normal2.dot(dir); if (dp < 0.0) { if (verbose) ROS_INFO("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) ROS_INFO("Visibility constraint is violated because the range angle is %lf (above the maximum allowed of %lf)", ang, max_view_angle_); return ConstraintEvaluationResult(false, 0.0); } } } shapes::Mesh *m = getVisibilityCone(state); if (!m) return ConstraintEvaluationResult(false, 0.0); // add the visibility cone as an object collision_detection::CollisionWorldFCL collision_world; collision_world.addToObject("cone", shapes::ShapeConstPtr(m), Eigen::Affine3d::Identity()); // check for collisions between the robot and the cone collision_detection::CollisionRequest req; collision_detection::CollisionResult res; collision_detection::AllowedCollisionMatrix acm; acm.setDefaultEntry("cone", boost::bind(&VisibilityConstraint::decideContact, this, _1)); req.contacts = true; req.verbose = verbose; req.max_contacts = 1; collision_world.checkRobotCollision(req, res, *collision_robot_, state, acm); if (verbose) { std::stringstream ss; m->print(ss); ROS_INFO("Visibility constraint %ssatisfied. Visibility cone approximation:\n %s", res.collision ? "not " : "", ss.str().c_str()); } return ConstraintEvaluationResult(!res.collision, res.collision ? res.contacts.begin()->second.front().depth : 0.0); }