// show the state of the current query
void collision_detection::CollisionRobotDistanceField::dumpQuery(const WorkArea& work, const char *descrip) const
{
logInform("CollisionRobotDistanceField Query %s", descrip);
std::stringstream ss_cost;
ss_cost << ", cost(max=" << work.req_->max_cost_sources << ", dens=" << work.req_->min_cost_density << ")";
std::stringstream ss_contacts;
ss_contacts << ", contact(max=" << work.req_->max_contacts << ", cpp=" << work.req_->max_contacts_per_pair << ")";
std::stringstream ss_acm;
if (work.acm_)
{
std::vector<std::string> names;
work.acm_->getAllEntryNames(names);
ss_acm << ", acm(nnames=" << names.size() << ", sz=" << work.acm_->getSize() << ")";
}
if (0)
{
logInform(" request: result%s%s%s%s%s%s",
work.req_->distance ? ", distance" : "",
work.req_->cost ? ss_cost.str().c_str() : "",
work.req_->contacts ? ss_contacts.str().c_str() : "",
work.req_->is_done ? ", is_done-func" : "",
work.req_->verbose ? ", VERBOSE" : "",
ss_acm.str().c_str());
}
}
void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std::string &path)
{
constraint_approximations_.clear();
std::ifstream fin((path + "/manifest").c_str());
if (!fin.good())
{
logDebug("Manifest not found in folder '%s'. Not loading constraint approximations.", path.c_str());
return;
}
logInform("Loading constrained space approximations from '%s'", path.c_str());
while (fin.good() && !fin.eof())
{
std::string group, state_space_parameterization, serialization, filename;
fin >> group;
if (fin.eof())
break;
fin >> state_space_parameterization;
if (fin.eof())
break;
fin >> serialization;
if (fin.eof())
break;
fin >> filename;
const ModelBasedPlanningContextPtr &pc = context_manager_.getPlanningContext(group, state_space_parameterization);
if (pc)
{
moveit_msgs::Constraints msg;
hexToMsg(serialization, msg);
ConstraintApproximationStateStorage *cass = new ConstraintApproximationStateStorage(pc->getOMPLSimpleSetup().getStateSpace());
cass->load((path + "/" + filename).c_str());
ConstraintApproximationPtr cap;
if (constraint_factories_.find(msg.name) != constraint_factories_.end())
cap = constraint_factories_[msg.name]->allocApproximation(context_manager_.getRobotModel(),
group, state_space_parameterization, msg, filename, ompl::base::StateStoragePtr(cass));
else
cap.reset(new ConstraintApproximation(context_manager_.getRobotModel(),
group, state_space_parameterization, msg, filename, ompl::base::StateStoragePtr(cass)));
if (cap)
{
if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end())
logWarn("Overwriting constraint approximation named '%s'", cap->getName().c_str());
constraint_approximations_[cap->getName()] = cap;
std::size_t sum = 0;
for (std::size_t i = 0 ; i < cass->size() ; ++i)
sum += cass->getMetadata(i).size();
logInform("Loaded %lu states and %lu connections (%0.1lf per state) from %s", cass->size(), sum, (double)sum / (double)cass->size(), filename.c_str());
}
}
}
}
// 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)
{
double dx = desired.x() - pt.x();
double dy = desired.y() - pt.y();
double dz = desired.z() - pt.z();
if (verbose)
{
logInform("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());
logInform("Differences %g %g %g",dx,dy, dz);
}
return ConstraintEvaluationResult(result, weight * sqrt(dx * dx + dy * dy + dz * dz));
}
void ompl_interface::ConstraintsLibrary::saveConstraintApproximations(const std::string &path)
{
logInform("Saving %u constrained space approximations to '%s'", (unsigned int)constraint_approximations_.size(), path.c_str());
try
{
boost::filesystem::create_directories(path);
}
catch(...)
{
}
std::ofstream fout((path + "/manifest").c_str());
if (fout.good())
for (std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.begin() ; it != constraint_approximations_.end() ; ++it)
{
fout << it->second->getGroup() << std::endl;
fout << it->second->getStateSpaceParameterization() << std::endl;
std::string serialization;
msgToHex(it->second->getConstraintsMsg(), serialization);
fout << serialization << std::endl;
fout << it->second->getFilename() << std::endl;
if (it->second->getStateStorage())
it->second->getStateStorage()->store((path + "/" + it->second->getFilename()).c_str());
}
else
logError("Unable to save constraint approximation to '%s'", path.c_str());
fout.close();
}
void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2,
const CollisionRobot &other_robot, const robot_state::RobotState &other_state1, const robot_state::RobotState &other_state2) const
{
res.collision = false;
if (req.verbose)
logInform("Using AllValid collision detection. No collision checking is performed.");
}
void moveit::tools::Profiler::console(void)
{
std::stringstream ss;
ss << std::endl;
status(ss, true);
logInform(ss.str().c_str());
}
moveit_rviz_plugin::PerLinkSubProperty::PerLinkSubProperty(
PerLinkProperty* parent,
PerLinkSubObjBase* sub_obj)
: PropertyHolder(sub_obj, parent)
, parent_(parent)
, sub_obj_(sub_obj)
, modified_(false)
{
logInform(" construct PerLinkSubProperty %s at %d",name_.c_str(), __LINE__);
sub_obj_->addProp(this);
if (flags_ & PF_NOT_PERLINK)
property_->hide();
}
moveit_rviz_plugin::PropertyHolder::PropertyHolder(
rviz::Property *parent_property,
const PropertyHolder* clone)
: name_(clone->name_)
, type_(clone->type_)
, flags_(clone->flags_)
, property_(NULL)
{
logInform(" construct PropertyHolder %s at %d (clone)",name_.c_str(), __LINE__);
createProperty(parent_property,
clone->property_->getDescription().toStdString(),
clone->property_->getValue());
}
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));
}
moveit_rviz_plugin::PerLinkProperty::PerLinkProperty(
PerLinkObjBase* obj,
const std::string& name,
const std::string& descr,
PropertyType type,
QVariant value,
unsigned int flags)
: PropertyHolder(obj, name, descr, type, value, flags)
, obj_(obj)
{
logInform(" construct PerLinkProperty %s at %d",name.c_str(), __LINE__);
obj_->addProp(this);
if (flags_ & PF_NOT_GLOBAL)
property_->hide();
}
moveit_rviz_plugin::PropertyHolder::PropertyHolder(
rviz::Property *parent_property,
const std::string& name,
const std::string& descr,
PropertyType type,
QVariant value,
unsigned int flags)
: name_(name)
, type_(type)
, flags_(flags)
, property_(NULL)
{
logInform(" construct PropertyHolder %s at %d (full)",name_.c_str(), __LINE__);
createProperty(parent_property, descr, value);
}
void moveit_rviz_plugin::PropertyHolder::createProperty(
rviz::Property *parent_property,
const std::string& descr,
QVariant value)
{
switch(type_)
{
case PT_EMPTY:
property_ = new rviz::Property(
name_.c_str(),
QVariant(),
descr.c_str(),
parent_property);
break;
case PT_INT:
property_ = new rviz::IntProperty(
name_.c_str(),
value.toInt(),
descr.c_str(),
parent_property,
SLOT( changedSlot() ),
this );
break;
case PT_FLOAT:
property_ = new rviz::FloatProperty(
name_.c_str(),
value.toDouble(),
descr.c_str(),
parent_property,
SLOT( changedSlot() ),
this );
break;
case PT_BOOL:
default:
type_ = PT_BOOL;
property_ = new rviz::BoolProperty(
name_.c_str(),
value.toBool(),
descr.c_str(),
parent_property,
SLOT( changedSlot() ),
this );
break;
}
logInform(" createProperty %08lx",long(property_));
if (flags_ & PF_READ_ONLY)
property_->setReadOnly(true);
}
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)));
}
TEST_F(FclCollisionDetectionTester, TestCollisionMapAdditionSpeed)
{
EigenSTL::vector_Affine3d poses;
std::vector<shapes::ShapeConstPtr> shapes;
for(unsigned int i = 0; i < 10000; i++) {
poses.push_back(Eigen::Affine3d::Identity());
shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01)));
}
ros::WallTime start = ros::WallTime::now();
cworld_->getWorld()->addToObject("map", shapes, poses);
double t = (ros::WallTime::now()-start).toSec();
EXPECT_GE(1.0, t);
// this is not really a failure; it is just that slow;
// looking into doing collision checking with a voxel grid.
logInform("Adding boxes took %g", t);
}
ompl_interface::ConstraintApproximationConstructionResults
ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::Constraints &constr_sampling, const moveit_msgs::Constraints &constr_hard,
const std::string &group, const std::string &state_space_parameterization,
const planning_scene::PlanningSceneConstPtr &scene, unsigned int samples, unsigned int edges_per_sample)
{
ConstraintApproximationConstructionResults res;
ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, state_space_parameterization);
if (pc)
{
pc->clear();
pc->setPlanningScene(scene);
pc->setCompleteInitialState(scene->getCurrentState());
std::map<std::string, ConstraintApproximationFactoryPtr>::const_iterator it = constraint_factories_.find(constr_hard.name);
ConstraintApproximationFactory *fct = NULL;
ConstraintStateStorageOrderFn order;
if (it != constraint_factories_.end())
{
fct = it->second.get();
order = fct->getOrderFunction();
}
ros::WallTime start = ros::WallTime::now();
ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, order, samples, edges_per_sample, res);
logInform("Spent %lf seconds constructing the database", (ros::WallTime::now() - start).toSec());
if (ss)
{
ConstraintApproximationPtr ca;
if (fct)
ca = fct->allocApproximation(context_manager_.getRobotModel(), group, state_space_parameterization, constr_hard, group + "_" +
boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + ".ompldb", ss);
else
ca.reset(new ConstraintApproximation(context_manager_.getRobotModel(), group, state_space_parameterization, constr_hard, group + "_" +
boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + ".ompldb", ss));
if (ca)
{
if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end())
logWarn("Overwriting constraint approximation named '%s'", ca->getName().c_str());
constraint_approximations_[ca->getName()] = ca;
res.approx = ca;
}
}
else
logError("Unable to construct constraint approximation for group '%s'", group.c_str());
}
return res;
}
void collision_detection::StaticDistanceField::initialize(
const bodies::Body& body,
double resolution,
double space_around_body,
bool save_points)
{
points_.clear();
inv_twice_resolution_ = 1.0 / (2.0 * resolution);
logInform(" create points at res=%f",resolution);
EigenSTL::vector_Vector3d points;
determineCollisionPoints(body, resolution, points);
if (points.empty())
{
logWarn(" StaticDistanceField::initialize: No points in body. Using origin.");
points.push_back(body.getPose().translation());
if (body.getType() == shapes::MESH)
{
const bodies::ConvexMesh& mesh = dynamic_cast<const bodies::ConvexMesh&>(body);
const EigenSTL::vector_Vector3d& verts = mesh.getVertices();
logWarn(" StaticDistanceField::initialize: also using %d vertices.", int(verts.size()));
EigenSTL::vector_Vector3d::const_iterator it = verts.begin();
EigenSTL::vector_Vector3d::const_iterator it_end = verts.end();
for ( ; it != it_end ; ++it)
{
points.push_back(*it);
}
}
}
logInform(" StaticDistanceField::initialize: Using %d points.", points.size());
AABB aabb;
aabb.add(points);
logInform(" space_around_body = %f",space_around_body);
logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (pre-space)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
aabb.max_.x(),
aabb.max_.y(),
aabb.max_.z());
aabb.min_ -= Eigen::Vector3d(space_around_body, space_around_body, space_around_body);
aabb.max_ += Eigen::Vector3d(space_around_body, space_around_body, space_around_body);
logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (pre-adjust)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
aabb.max_.x(),
aabb.max_.y(),
aabb.max_.z());
aabb.min_.x() = std::floor(aabb.min_.x() / resolution) * resolution;
aabb.min_.y() = std::floor(aabb.min_.y() / resolution) * resolution;
aabb.min_.z() = std::floor(aabb.min_.z() / resolution) * resolution;
logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (post-adjust)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
aabb.max_.x(),
aabb.max_.y(),
aabb.max_.z());
Eigen::Vector3d size = aabb.max_ - aabb.min_;
double diagonal = size.norm();
logInform(" DF: sz=(%7.3f %7.3f %7.3f) cnt=(%d %d %d) diag=%f",
size.x(),
size.y(),
size.z(),
int(size.x()/resolution),
int(size.y()/resolution),
int(size.z()/resolution),
diagonal);
distance_field::PropagationDistanceField df(
size.x(),
size.y(),
size.z(),
resolution,
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
diagonal * 2.0,
true);
df.addPointsToField(points);
DistPosEntry default_entry;
default_entry.distance_ = diagonal * 2.0;
default_entry.cell_id_ = -1;
resize(size.x(),
size.y(),
size.z(),
resolution,
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
default_entry);
logInform(" copy %d points.",
getNumCells(distance_field::DIM_X) *
getNumCells(distance_field::DIM_Y) *
getNumCells(distance_field::DIM_Z));
int pdf_x,pdf_y,pdf_z;
int sdf_x,sdf_y,sdf_z;
Eigen::Vector3d pdf_p, sdf_p;
df.worldToGrid(aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), pdf_x,pdf_y,pdf_z);
worldToGrid(aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), sdf_x,sdf_y,sdf_z);
df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());
logInform(" DF: min=(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: min=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)",
pdf_p.x(),
pdf_p.y(),
pdf_p.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: min=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)",
sdf_p.x(),
sdf_p.y(),
sdf_p.z(),
sdf_x,
sdf_y,
sdf_z);
df.worldToGrid(0,0,0, pdf_x,pdf_y,pdf_z);
worldToGrid(0,0,0, sdf_x,sdf_y,sdf_z);
df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());
logInform(" DF: org=(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)",
0.0,
0.0,
0.0,
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: org=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)",
pdf_p.x(),
pdf_p.y(),
pdf_p.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: org=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)",
sdf_p.x(),
sdf_p.y(),
sdf_p.z(),
sdf_x,
sdf_y,
sdf_z);
df.worldToGrid(points[0].x(), points[0].y(), points[0].z(), pdf_x,pdf_y,pdf_z);
worldToGrid(points[0].x(), points[0].y(), points[0].z(), sdf_x,sdf_y,sdf_z);
df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());
logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)",
points[0].x(),
points[0].y(),
points[0].z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)",
pdf_p.x(),
pdf_p.y(),
pdf_p.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)",
sdf_p.x(),
sdf_p.y(),
sdf_p.z(),
sdf_x,
sdf_y,
sdf_z);
for (int z = 0 ; z < df.getZNumCells() ; ++z)
{
for (int y = 0 ; y < df.getYNumCells() ; ++y)
{
for (int x = 0 ; x < df.getXNumCells() ; ++x)
{
DistPosEntry entry;
double dist = df.getDistance(x, y, z);
const distance_field::PropDistanceFieldVoxel& voxel = df.getCell(x,y,z);
if (dist < 0)
{
// propogation distance field has a bias of -1*resolution on points inside the object
if (dist <= -resolution)
{
dist += resolution;
}
else
{
logError("PropagationDistanceField returned distance=%f between 0 and -resolution=%f."
" Did someone fix it?"
" Need to remove workaround from static_distance_field.cpp",
dist,-resolution);
dist = 0.0;
}
entry.distance_ = dist;
entry.cell_id_ = getCellId(
voxel.closest_negative_point_.x(),
voxel.closest_negative_point_.y(),
voxel.closest_negative_point_.z());
}
else
{
entry.distance_ = dist;
entry.cell_id_ = getCellId(
voxel.closest_point_.x(),
voxel.closest_point_.y(),
voxel.closest_point_.z());
}
setCell(x, y, z, entry);
}
}
}
if (save_points)
std::swap(points, points_);
}
ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstraintApproximation(const ModelBasedPlanningContextPtr &pcontext,
const moveit_msgs::Constraints &constr_sampling,
const moveit_msgs::Constraints &constr_hard,
const ConstraintStateStorageOrderFn &order,
unsigned int samples, unsigned int edges_per_sample,
ConstraintApproximationConstructionResults &result)
{
// state storage structure
ConstraintApproximationStateStorage *cass = new ConstraintApproximationStateStorage(pcontext->getOMPLStateSpace());
ob::StateStoragePtr sstor(cass);
// construct a sampler for the sampling constraints
kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel(), robot_state::TransformsConstPtr(new robot_state::Transforms(pcontext->getRobotModel()->getModelFrame())));
kset.add(constr_hard);
const robot_state::RobotState &default_state = pcontext->getCompleteInitialRobotState();
int nthreads = 0;
unsigned int attempts = 0;
double bounds_val = std::numeric_limits<double>::max() / 2.0 - 1.0;
pcontext->getOMPLStateSpace()->setPlanningVolume(-bounds_val, bounds_val, -bounds_val, bounds_val, -bounds_val, bounds_val);
pcontext->getOMPLStateSpace()->setup();
// construct the constrained states
#pragma omp parallel
{
#pragma omp master
{
nthreads = omp_get_num_threads();
}
robot_state::RobotState kstate(default_state);
const constraint_samplers::ConstraintSamplerManagerPtr &csmng = pcontext->getConstraintSamplerManager();
ConstrainedSampler *csmp = NULL;
if (csmng)
{
constraint_samplers::ConstraintSamplerPtr cs = csmng->selectSampler(pcontext->getPlanningScene(), pcontext->getJointModelGroup()->getName(), constr_sampling);
if (cs)
csmp = new ConstrainedSampler(pcontext.get(), cs);
}
ob::StateSamplerPtr ss(csmp ? ob::StateSamplerPtr(csmp) : pcontext->getOMPLStateSpace()->allocDefaultStateSampler());
ompl::base::ScopedState<> temp(pcontext->getOMPLStateSpace());
int done = -1;
bool slow_warn = false;
ompl::time::point start = ompl::time::now();
while (sstor->size() < samples)
{
++attempts;
#pragma omp master
{
int done_now = 100 * sstor->size() / samples;
if (done != done_now)
{
done = done_now;
logInform("%d%% complete (kept %0.1lf%% sampled states)", done, 100.0 * (double)sstor->size() / (double)attempts);
}
if (!slow_warn && attempts > 10 && attempts > sstor->size() * 100)
{
slow_warn = true;
logWarn("Computation of valid state database is very slow...");
}
}
if (attempts > samples && sstor->size() == 0)
{
logError("Unable to generate any samples");
break;
}
ss->sampleUniform(temp.get());
pcontext->getOMPLStateSpace()->copyToRobotState(kstate, temp.get());
if (kset.decide(kstate).satisfied)
{
#pragma omp critical
{
if (sstor->size() < samples)
{
temp->as<ModelBasedStateSpace::StateType>()->tag = sstor->size();
sstor->addState(temp.get());
}
}
}
}
#pragma omp master
{
result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start);
logInform("Generated %u states in %lf seconds", (unsigned int)sstor->size(), result.state_sampling_time);
if (csmp)
{
result.sampling_success_rate = csmp->getConstrainedSamplingRate();
logInform("Constrained sampling rate: %lf", result.sampling_success_rate);
}
}
}
if (order)
{
logInform("Sorting states...");
sstor->sort(order);
}
if (edges_per_sample > 0)
{
logInform("Computing graph connections (max %u edges per sample) ...", edges_per_sample);
ompl::tools::SelfConfig sc(pcontext->getOMPLSimpleSetup().getSpaceInformation());
double range = 0.0;
sc.configurePlannerRange(range);
// construct connexions
const ob::StateSpacePtr &space = pcontext->getOMPLSimpleSetup().getStateSpace();
std::vector<robot_state::RobotState> kstates(nthreads, default_state);
const std::vector<const ompl::base::State*> &states = sstor->getStates();
std::vector<ompl::base::ScopedState<> > temps(nthreads, ompl::base::ScopedState<>(space));
ompl::time::point start = ompl::time::now();
int good = 0;
int done = -1;
#pragma omp parallel for schedule(dynamic)
for (std::size_t j = 0 ; j < sstor->size() ; ++j)
{
int threadid = omp_get_thread_num();
robot_state::RobotState &kstate = kstates[threadid];
robot_state::JointStateGroup *jsg = kstate.getJointStateGroup(pcontext->getJointModelGroup()->getName());
ompl::base::State *temp = temps[threadid].get();
int done_now = 100 * j / sstor->size();
if (done != done_now)
{
done = done_now;
logInform("%d%% complete", done);
}
for (std::size_t i = j + 1 ; i < sstor->size() ; ++i)
{
double d = space->distance(states[j], states[i]);
if (d > range * 3.0 || d < range / 100.0)
continue;
space->interpolate(states[j], states[i], 0.5, temp);
pcontext->getOMPLStateSpace()->copyToRobotState(kstate, temp);
if (kset.decide(kstate).satisfied)
{
space->interpolate(states[j], states[i], 0.25, temp);
pcontext->getOMPLStateSpace()->copyToRobotState(kstate, temp);
if (kset.decide(kstate).satisfied)
{
space->interpolate(states[j], states[i], 0.75, temp);
pcontext->getOMPLStateSpace()->copyToRobotState(kstate, temp);
if (kset.decide(kstate).satisfied)
{
#pragma omp critical
{
cass->getMetadata(i).push_back(j);
cass->getMetadata(j).push_back(i);
good++;
}
if (cass->getMetadata(j).size() >= edges_per_sample)
break;
}
}
}
}
}
result.state_connection_time = ompl::time::seconds(ompl::time::now() - start);
logInform("Computed possible connexions in %lf seconds. Added %d connexions", result.state_connection_time, good);
}
return sstor;
}
void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const
{
res.collision = false;
if (req.verbose)
logInform("Using AllValid collision detection. No collision checking is performed.");}
bool parseJoint(Joint &joint, TiXmlElement* config)
{
joint.clear();
// Get Joint Name
const char *name = config->Attribute("name");
if (!name)
{
logError("unnamed joint found");
return false;
}
joint.name = name;
// Get transform from Parent Link to Joint Frame
TiXmlElement *origin_xml = config->FirstChildElement("origin");
if (!origin_xml)
{
logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str());
joint.parent_to_joint_origin_transform.clear();
}
else
{
if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml))
{
joint.parent_to_joint_origin_transform.clear();
logError("Malformed parent origin element for joint [%s]", joint.name.c_str());
return false;
}
}
// Get Parent Link
TiXmlElement *parent_xml = config->FirstChildElement("parent");
if (parent_xml)
{
const char *pname = parent_xml->Attribute("link");
if (!pname)
{
logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str());
}
else
{
joint.parent_link_name = std::string(pname);
}
}
// Get Child Link
TiXmlElement *child_xml = config->FirstChildElement("child");
if (child_xml)
{
const char *pname = child_xml->Attribute("link");
if (!pname)
{
logInform("no child link name specified for Joint link [%s].", joint.name.c_str());
}
else
{
joint.child_link_name = std::string(pname);
}
}
// Get Joint type
const char* type_char = config->Attribute("type");
if (!type_char)
{
logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str());
return false;
}
std::string type_str = type_char;
if (type_str == "planar")
joint.type = Joint::PLANAR;
else if (type_str == "floating")
joint.type = Joint::FLOATING;
else if (type_str == "revolute")
joint.type = Joint::REVOLUTE;
else if (type_str == "continuous")
joint.type = Joint::CONTINUOUS;
else if (type_str == "prismatic")
joint.type = Joint::PRISMATIC;
else if (type_str == "fixed")
joint.type = Joint::FIXED;
else
{
logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str());
return false;
}
// Get Joint Axis
if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
if (!axis_xml){
logDebug("urdfdom: no axis elemement for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str());
joint.axis = Vector3(1.0, 0.0, 0.0);
}
else{
if (axis_xml->Attribute("xyz")){
try {
joint.axis.init(axis_xml->Attribute("xyz"));
}
catch (ParseError &e) {
joint.axis.clear();
logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what());
return false;
}
}
}
}
// Get limit
TiXmlElement *limit_xml = config->FirstChildElement("limit");
if (limit_xml)
{
joint.limits.reset(new JointLimits());
if (!parseJointLimits(*joint.limits, limit_xml))
{
logError("Could not parse limit element for joint [%s]", joint.name.c_str());
joint.limits.reset();
return false;
}
}
else if (joint.type == Joint::REVOLUTE)
{
logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str());
return false;
}
else if (joint.type == Joint::PRISMATIC)
{
logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str());
return false;
}
// Get safety
TiXmlElement *safety_xml = config->FirstChildElement("safety_controller");
if (safety_xml)
{
joint.safety.reset(new JointSafety());
if (!parseJointSafety(*joint.safety, safety_xml))
{
logError("Could not parse safety element for joint [%s]", joint.name.c_str());
joint.safety.reset();
return false;
}
}
// Get calibration
TiXmlElement *calibration_xml = config->FirstChildElement("calibration");
if (calibration_xml)
{
joint.calibration.reset(new JointCalibration());
if (!parseJointCalibration(*joint.calibration, calibration_xml))
{
logError("Could not parse calibration element for joint [%s]", joint.name.c_str());
joint.calibration.reset();
return false;
}
}
// Get Joint Mimic
TiXmlElement *mimic_xml = config->FirstChildElement("mimic");
if (mimic_xml)
{
joint.mimic.reset(new JointMimic());
if (!parseJointMimic(*joint.mimic, mimic_xml))
{
logError("Could not parse mimic element for joint [%s]", joint.name.c_str());
joint.mimic.reset();
return false;
}
}
// Get Dynamics
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
if (prop_xml)
{
joint.dynamics.reset(new JointDynamics());
if (!parseJointDynamics(*joint.dynamics, prop_xml))
{
logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str());
joint.dynamics.reset();
return false;
}
}
return true;
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::VisibilityConstraint::decide(const robot_state::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)
{
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_;
//necessary to do subtraction as SENSOR_Z is 0 and SENSOR_X is 2
const Eigen::Vector3d &normal2 = sp.rotation().col(2-sensor_view_direction_);
if (max_view_angle_ > 0.0)
{
const Eigen::Vector3d &normal1 = tp.rotation().col(2)*-1.0; // along Z axis and inverted
double dp = normal2.dot(normal1);
double ang = acos(dp);
if (dp < 0.0)
{
if (verbose)
logInform("Visibility constraint is violated because the sensor is looking at the wrong side");
return ConstraintEvaluationResult(false, 0.0);
}
if (max_view_angle_ < ang)
{
if (verbose)
logInform("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)
logInform("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)
logInform("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);
// add the visibility cone as an object
collision_detection::CollisionWorldFCL collision_world;
collision_world.getWorld()->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);
logInform("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);
}
bool mesh_core::LineSegment2D::intersect(
const LineSegment2D& other,
Eigen::Vector2d& intersection,
bool& parallel) const
{
const LineSegment2D& a = *this;
const LineSegment2D& b = other;
if (acorn_debug_ear_state)
{
logInform("Intersect");
logInform(" a=%s",a.str().c_str());
logInform(" b=%s",b.str().c_str());
}
if (a.vertical_)
{
if (b.vertical_)
{
parallel = true;
intersection = a.pt_[0];
if (a.pt_[0].x() != b.pt_[0].x())
return false;
double aymin = std::min(a.pt_[0].y(), a.pt_[1].y());
double aymax = std::max(a.pt_[0].y(), a.pt_[1].y());
double bymin = std::min(b.pt_[0].y(), b.pt_[1].y());
double bymax = std::max(b.pt_[0].y(), b.pt_[1].y());
if (acorn_debug_ear_state)
{
logInform(" both vertical aymin=%f aymax=%f bymin=%f bymax\%f",
aymin, aymax, bymin, bymax);
}
if (bymax < aymin)
return false;
if (bymin > aymax)
return false;
if (bymax <= aymax)
intersection.y() = bymax;
else if (bymin >= aymin)
intersection.y() = bymin;
else
intersection.y() = aymin;
if (acorn_debug_ear_state)
{
logInform(" INTERSECTS");
}
return true;
}
parallel = false;
intersection.x() = a.pt_[0].x();
intersection.y() = b.slope_ * intersection.x() + b.y_intercept_;
double tb = (intersection.x() - b.pt_[0].x()) * b.inv_dx_;
if (acorn_debug_ear_state)
{
logInform(" a vertical pt=(%8.4f, %8.4f) tb=%f",
intersection.x(),
intersection.y(),
tb);
}
if (tb > 1.0 || tb < 0.0)
return false;
if (b.inv_dx_ == 0.0)
{
if (intersection.y() != a.pt_[0].y())
return false;
}
else
{
double ta = (intersection.y() - a.pt_[0].y()) * a.inv_dx_;
if (acorn_debug_ear_state)
{
logInform(" ta=%f", ta);
}
if (ta > 1.0 || ta < 0.0)
return false;
}
if (acorn_debug_ear_state)
{
logInform(" INTERSECTS");
}
return true;
}
else if (b.vertical_)
{
return b.intersect(a, intersection, parallel);
}
else
{
double bottom = a.slope_ - b.slope_;
if (std::abs(bottom) < std::numeric_limits<double>::epsilon())
{
parallel = true;
intersection.setZero();
return false;
}
parallel = false;
intersection.x() = (b.y_intercept_ - a.y_intercept_) / bottom;
intersection.y() = a.slope_ * intersection.x() + a.y_intercept_;
double ta = (intersection.x() - a.pt_[0].x()) * a.inv_dx_;
if (acorn_debug_ear_state)
{
logInform(" not vertical pt=(%8.4f, %8.4f) ta=%f",
intersection.x(),
intersection.y(),
ta);
}
if (ta > 1.0 || ta < 0.0)
return false;
double tb = (intersection.x() - b.pt_[0].x()) * b.inv_dx_;
if (acorn_debug_ear_state)
{
logInform(" tb=%f", tb);
}
if (tb > 1.0 || tb < 0.0)
return false;
if (acorn_debug_ear_state)
{
logInform(" INTERSECTS");
}
return true;
}
}
// find the tightest bounding sphere.
// Recursive.
// The first nbound points in list_ are known to be on the boundary. The rest need to be checked.
void SphereInfo::findSphere(
int nbound,
int npoints)
{
// increase radius by this much to avoid flipping between points that are
// equal distance from center.
static const double radius_expand =
std::numeric_limits<float>::epsilon() * 1000.0;
if (g_verbose)
{
logInform(" findSphere(%d,%3d) ENTER",
nbound,npoints);
}
switch(nbound)
{
case 0:
center_ = Eigen::Vector3d::Zero();
radius_ = 0;
break;
case 1:
center_ = *list_[0];
radius_ = radius_expand;
break;
case 2:
mesh_core::findSphereTouching2Points(
center_,
radius_,
*list_[0],
*list_[1]);
radius_ += radius_expand;
break;
case 3:
mesh_core::findSphereTouching3Points(
center_,
radius_,
*list_[0],
*list_[1],
*list_[2]);
radius_ += radius_expand;
break;
default:
logError("Bad nbound=%d for findSphere",nbound);
case 4:
mesh_core::findSphereTouching4Points(
center_,
radius_,
*list_[0],
*list_[1],
*list_[2],
*list_[3]);
radius_ += radius_expand;
radius_sq_ = radius_ * radius_;
if (g_verbose)
{
logInform(" findSphere(%d,%4d) begin check s = (%7.3f %7.3f %7.3f) r=%10.6f rsq=%10.6f ",
nbound,
npoints,
center_.x(),
center_.y(),
center_.z(),
radius_,
radius_sq_);
for (int i=0; i<nbound; ++i)
{
double d = (*list_[i] - center_).norm();
logInform(" BOUND: list_[%4d] = %4d (%7.3f %7.3f %7.3f) d=%10.6f (%10.6f) (BOUND) ",
i,
int(list_[i] - &points_[0]),
list_[i]->x(),
list_[i]->y(),
list_[i]->z(),
d,
radius_ - d);
}
}
return;
}
radius_sq_ = radius_ * radius_;
if (g_verbose)
{
logInform(" findSphere(%d,%4d) begin check s = (%7.3f %7.3f %7.3f) r=%10.6f rsq=%10.6f ",
nbound,
npoints,
center_.x(),
center_.y(),
center_.z(),
radius_,
radius_sq_);
for (int i=0; i<nbound; ++i)
{
double d = (*list_[i] - center_).norm();
logInform(" BOUND: list_[%4d] = %4d (%7.3f %7.3f %7.3f) d=%10.6f (%10.6f) (BOUND) ",
i,
int(list_[i] - &points_[0]),
list_[i]->x(),
list_[i]->y(),
list_[i]->z(),
d,
radius_ - d);
}
}
for (int i = nbound ; i < npoints ; ++i)
{
if (g_verbose)
{
double d = (*list_[i] - center_).norm();
logInform(" check list_[%4d] = %4d (%7.3f %7.3f %7.3f) d=%10.6f (%10.6f) (%s) ",
i,
int(list_[i] - &points_[0]),
list_[i]->x(),
list_[i]->y(),
list_[i]->z(),
d,
radius_ - d,
((*list_[i] - center_).squaredNorm() > radius_sq_) ? "OUTSIDE" : "ok");
}
if ((*list_[i] - center_).squaredNorm() > radius_sq_)
{
// entry i is a troublemaker, so move it to head of list (i.e. to
// list_[nbound])
const Eigen::Vector3d *p = list_[i];
if (g_verbose)
{
logInform(" Point %4d outside -- move to list_[%4d] ",
int(list_[i] - &points_[0]),
nbound);
}
memmove(&list_[nbound+1],
&list_[nbound],
sizeof(const Eigen::Vector3d*) * (i - nbound));
list_[nbound] = p;
// find a sphere that has the new boundary point and contains all points that have already been checked so far.
findSphere(nbound + 1, i + 1);
if (g_verbose)
{
logInform(" findSphere(%d,%4d) cont s = (%7.3f %7.3f %7.3f) r=%10.6f rsq=%10.6f ",
nbound,
npoints,
center_.x(),
center_.y(),
center_.z(),
radius_,
radius_sq_);
}
}
}
if (g_verbose)
{
logInform(" findSphere(%d,%4d) RETURN s = (%7.3f %7.3f %7.3f) r=%10.6f rsq=%10.6f ",
nbound,
npoints,
center_.x(),
center_.y(),
center_.z(),
radius_,
radius_sq_);
}
}
TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler)
{
moveit_msgs::Constraints c;
moveit_msgs::PositionConstraint pcm;
pcm.link_name = "l_wrist_roll_link";
pcm.target_point_offset.x = 0;
pcm.target_point_offset.y = 0;
pcm.target_point_offset.z = 0;
pcm.constraint_region.primitives.resize(1);
pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
pcm.constraint_region.primitives[0].dimensions.resize(1);
pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
pcm.header.frame_id = kmodel->getModelFrame();
pcm.constraint_region.primitive_poses.resize(1);
pcm.constraint_region.primitive_poses[0].position.x = 0.55;
pcm.constraint_region.primitive_poses[0].position.y = 0.2;
pcm.constraint_region.primitive_poses[0].position.z = 1.25;
pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
pcm.weight = 1.0;
c.position_constraints.push_back(pcm);
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "l_wrist_roll_link";
ocm.header.frame_id = kmodel->getModelFrame();
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.2;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.4;
ocm.weight = 1.0;
c.orientation_constraints.push_back(ocm);
ocm.link_name = "r_wrist_roll_link";
ocm.header.frame_id = kmodel->getModelFrame();
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.01;
ocm.absolute_y_axis_tolerance = 0.01;
ocm.absolute_z_axis_tolerance = 0.01;
ocm.weight = 1.0;
c.orientation_constraints.push_back(ocm);
robot_state::Transforms &tf = ps->getTransformsNonConst();
constraint_samplers::ConstraintSamplerPtr s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "arms", c);
EXPECT_TRUE(s);
constraint_samplers::UnionConstraintSampler* ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
EXPECT_TRUE(ucs);
kinematic_constraints::KinematicConstraintSet kset(kmodel);
kset.add(c, tf);
robot_state::RobotState ks(kmodel);
ks.setToDefaultValues();
ks.update();
robot_state::RobotState ks_const(kmodel);
ks_const.setToDefaultValues();
ks_const.update();
static const int NT = 100;
int succ = 0;
for (int t = 0 ; t < NT ; ++t)
{
EXPECT_TRUE(s->sample(ks, ks_const, 1000));
EXPECT_TRUE(kset.decide(ks).satisfied);
if (s->sample(ks, ks_const, 1))
succ++;
}
logInform("Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf", (double)succ / (double)NT);
}
void mesh_core::findSphereTouching4Points(
Eigen::Vector3d& center,
double& radius,
const Eigen::Vector3d& a,
const Eigen::Vector3d& b,
const Eigen::Vector3d& c,
const Eigen::Vector3d& d)
{
Eigen::Matrix3d m;
m.col(0) = b - a;
m.col(1) = c - a;
m.col(2) = d - a;
double det = m.determinant();
// points are coplanar?
if (std::abs(det) <= std::numeric_limits<double>::epsilon())
{
findSphereTouching4PointsCoplanar(center, radius, a,b,c,d);
if (g_verbose)
{
logInform("findSphereTouching4Points() COPLANAR QQQQ");
logInform(" a = (%7.3f %7.3f %7.3f)",
a.x(),
a.y(),
a.z());
logInform(" b = (%7.3f %7.3f %7.3f)",
b.x(),
b.y(),
b.z());
logInform(" c = (%7.3f %7.3f %7.3f)",
c.x(),
c.y(),
c.z());
logInform(" d = (%7.3f %7.3f %7.3f)",
d.x(),
d.y(),
d.z());
logInform(" s = (%7.3f %7.3f %7.3f) r=%7.3f",
center.x(),
center.y(),
center.z(),
radius);
}
return;
}
double ab_lensq = m.col(0).squaredNorm();
double ac_lensq = m.col(1).squaredNorm();
double ad_lensq = m.col(2).squaredNorm();
Eigen::Vector3d rel_center = ((ad_lensq * m.col(0).cross(m.col(1))) +
(ac_lensq * m.col(2).cross(m.col(0))) +
(ab_lensq * m.col(1).cross(m.col(2)))) /
(2.0 * det);
radius = rel_center.norm();
center = rel_center + a;
if (g_verbose)
{
logInform("findSphereTouching4Points()");
logInform(" a = (%7.3f %7.3f %7.3f)",
a.x(),
a.y(),
a.z());
logInform(" b = (%7.3f %7.3f %7.3f)",
b.x(),
b.y(),
b.z());
logInform(" c = (%7.3f %7.3f %7.3f)",
c.x(),
c.y(),
c.z());
logInform(" d = (%7.3f %7.3f %7.3f)",
d.x(),
d.y(),
d.z());
logInform(" s = (%7.3f %7.3f %7.3f) r=%7.3f",
center.x(),
center.y(),
center.z(),
radius);
}
}
// return closest point on triangle to the given point, and the distance
// betweeen them.
// If distance is greater than max_dist then skip the calculations and just return the approximate distance (anything greater than max_dist).
double mesh_core::closestPointOnTriangle(
const Eigen::Vector3d& tri_a,
const Eigen::Vector3d& tri_b,
const Eigen::Vector3d& tri_c,
const Eigen::Vector3d& point,
Eigen::Vector3d& closest_point,
double max_dist)
{
Eigen::Vector3d ab = tri_b - tri_a;
Eigen::Vector3d ac = tri_c - tri_a;
Eigen::Vector3d n = ab.cross(ac);
Eigen::Vector3d norm = n.normalized();
Eigen::Vector3d ap = point - tri_a;
double dist = norm.dot(ap);
double abs_dist = std::abs(dist);
if (abs_dist >= max_dist)
return abs_dist;
closest_point = point - dist * norm;
if (acorn_closest_debug)
{
logInform(" CDB: tri_a (%8.4f %8.4f %8.4f)",
tri_a.x(),
tri_a.y(),
tri_a.z());
logInform(" CDB: tri_b (%8.4f %8.4f %8.4f)",
tri_b.x(),
tri_b.y(),
tri_b.z());
logInform(" CDB: tri_c (%8.4f %8.4f %8.4f)",
tri_c.x(),
tri_c.y(),
tri_c.z());
logInform(" CDB: point (%8.4f %8.4f %8.4f)",
point.x(),
point.y(),
point.z());
logInform(" CDB: intersect (%8.4f %8.4f %8.4f)",
closest_point.x(),
closest_point.y(),
closest_point.z());
}
Eigen::Vector3d ab_norm = ab.cross(norm);
if (acorn_closest_debug)
{
logInform(" CDB: ab_norm (%8.4f %8.4f %8.4f) dp=%8.4f >? da=%8.4f",
ab_norm.x(),
ab_norm.y(),
ab_norm.z(),
ab_norm.dot(point),
ab_norm.dot(tri_a));
}
if (ab_norm.dot(point) > ab_norm.dot(tri_a))
{
if (acorn_closest_debug)
{
logInform(" CDB: beyond ab");
}
return closestPointOnLine(tri_a, tri_b, point, closest_point);
}
Eigen::Vector3d ac_norm = ac.cross(norm);
if (acorn_closest_debug)
{
logInform(" CDB: ac_norm (%8.4f %8.4f %8.4f) dp=%8.4f <? da=%8.4f",
ac_norm.x(),
ac_norm.y(),
ac_norm.z(),
ac_norm.dot(point),
ac_norm.dot(tri_a));
}
if (ac_norm.dot(point) < ac_norm.dot(tri_a))
{
if (acorn_closest_debug)
{
logInform(" CDB: beyond ac");
}
return closestPointOnLine(tri_a, tri_c, point, closest_point);
}
Eigen::Vector3d bc = tri_c - tri_b;
Eigen::Vector3d bc_norm = bc.cross(norm);
if (acorn_closest_debug)
{
logInform(" CDB: ab_norm (%8.4f %8.4f %8.4f) dp=%8.4f >? db=%8.4f",
bc_norm.x(),
bc_norm.y(),
bc_norm.z(),
bc_norm.dot(point),
bc_norm.dot(tri_b));
}
if (bc_norm.dot(point) > bc_norm.dot(tri_b))
{
if (acorn_closest_debug)
{
logInform(" CDB: beyond bc");
}
return closestPointOnLine(tri_b, tri_c, point, closest_point);
}
return abs_dist;
}
static void findSphereTouching4PointsCoplanar(
Eigen::Vector3d& center,
double& radius,
const Eigen::Vector3d& a,
const Eigen::Vector3d& b,
const Eigen::Vector3d& c,
const Eigen::Vector3d& d)
{
mesh_core::findSphereTouching3Points(center,
radius,
a,
b,
c);
if (g_verbose)
{
double dist = (center - d).norm();
logInform(" findSphereTouching4PointsCoplanar abc center=(%7.3f %7.3f %7.3f) r=%10.6f d.dist=%10.6f (%10.6f) (%s)",
center.x(),
center.y(),
center.z(),
radius,
dist,
radius - dist,
radius - dist < 0 ? "OUTSIDE" : "ok");
}
if ((center - d).squaredNorm() <= radius*radius)
return;
mesh_core::findSphereTouching3Points(center,
radius,
a,
b,
d);
if (g_verbose)
{
double dist = (center - c).norm();
logInform(" findSphereTouching4PointsCoplanar abd center=(%7.3f %7.3f %7.3f) r=%10.6f c.dist=%10.6f (%10.6f) (%s)",
center.x(),
center.y(),
center.z(),
radius,
dist,
radius - dist,
radius - dist < 0 ? "OUTSIDE" : "ok");
}
if ((center - c).squaredNorm() <= radius*radius)
return;
mesh_core::findSphereTouching3Points(center,
radius,
a,
c,
d);
if (g_verbose)
{
double dist = (center - b).norm();
logInform(" findSphereTouching4PointsCoplanar acd center=(%7.3f %7.3f %7.3f) r=%10.6f b.dist=%10.6f (%10.6f) (%s)",
center.x(),
center.y(),
center.z(),
radius,
dist,
radius - dist,
radius - dist < 0 ? "OUTSIDE" : "ok");
}
if ((center - b).squaredNorm() <= radius*radius)
return;
mesh_core::findSphereTouching3Points(center,
radius,
b,
c,
d);
if (g_verbose)
{
double dist = (center - a).norm();
logInform(" findSphereTouching4PointsCoplanar bcd center=(%7.3f %7.3f %7.3f) r=%10.6f a.dist=%10.6f (%10.6f) (%s)",
center.x(),
center.y(),
center.z(),
radius,
dist,
radius - dist,
radius - dist < 0 ? "OUTSIDE" : "ok");
}
}
ompl::base::PlannerStatus ompl::geometric::RRTConnect::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
if (!goal)
{
logError("Unknown type of goal (or goal undefined)");
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tStart_->add(motion);
}
if (tStart_->size() == 0)
{
logError("Motion planning start tree could not be initialized!");
return base::PlannerStatus::INVALID_START;
}
if (!goal->couldSample())
{
logError("Insufficient states in sampleable goal region");
return base::PlannerStatus::INVALID_GOAL;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
logInform("Starting with %d states", (int)(tStart_->size() + tGoal_->size()));
TreeGrowingInfo tgi;
tgi.xstate = si_->allocState();
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
bool startTree = true;
bool solved = false;
while (ptc() == false)
{
TreeData &tree = startTree ? tStart_ : tGoal_;
tgi.start = startTree;
startTree = !startTree;
TreeData &otherTree = startTree ? tStart_ : tGoal_;
if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
{
const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
{
Motion* motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tGoal_->add(motion);
}
if (tGoal_->size() == 0)
{
logError("Unable to sample any valid states for goal tree");
break;
}
}
/* sample random state */
sampler_->sampleUniform(rstate);
GrowState gs = growTree(tree, tgi, rmotion);
if (gs != TRAPPED)
{
/* remember which motion was just added */
Motion *addedMotion = tgi.xmotion;
/* attempt to connect trees */
/* if reached, it means we used rstate directly, no need top copy again */
if (gs != REACHED)
si_->copyState(rstate, tgi.xstate);
GrowState gsc = ADVANCED;
tgi.start = startTree;
while (gsc == ADVANCED)
gsc = growTree(otherTree, tgi, rmotion);
Motion *startMotion = startTree ? tgi.xmotion : addedMotion;
Motion *goalMotion = startTree ? addedMotion : tgi.xmotion;
/* if we connected the trees in a valid way (start and goal pair is valid)*/
if (gsc == REACHED && goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
{
// it must be the case that either the start tree or the goal tree has made some progress
// so one of the parents is not NULL. We go one step 'back' to avoid having a duplicate state
// on the solution path
if (startMotion->parent)
startMotion = startMotion->parent;
else
goalMotion = goalMotion->parent;
connectionPoint_ = std::make_pair<base::State*, base::State*>(startMotion->state, goalMotion->state);
/* construct the solution path */
Motion *solution = startMotion;
std::vector<Motion*> mpath1;
while (solution != NULL)
{
mpath1.push_back(solution);
solution = solution->parent;
}
solution = goalMotion;
std::vector<Motion*> mpath2;
while (solution != NULL)
{
mpath2.push_back(solution);
solution = solution->parent;
}
PathGeometric *path = new PathGeometric(si_);
path->getStates().reserve(mpath1.size() + mpath2.size());
for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
path->append(mpath1[i]->state);
for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
path->append(mpath2[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
solved = true;
break;
}
}
}
si_->freeState(tgi.xstate);
si_->freeState(rstate);
delete rmotion;
logInform("Created %u states (%u start + %u goal)", tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());
return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
void mesh_core::findSphereTouching3Points(
Eigen::Vector3d& center,
double& radius,
const Eigen::Vector3d& a,
const Eigen::Vector3d& b,
const Eigen::Vector3d& c)
{
Eigen::Vector3d ab = b - a;
Eigen::Vector3d ac = c - a;
Eigen::Vector3d n = ab.cross(ac);
double ab_lensq = ab.squaredNorm();
double ac_lensq = ac.squaredNorm();
double n_lensq = n.squaredNorm();
// points are colinear?
if (n_lensq <= std::numeric_limits<double>::epsilon())
{
findSphereTouching3PointsColinear(
center,
radius,
a,
b,
c,
ab_lensq,
(c - b).squaredNorm(),
ac_lensq);
if (g_verbose)
{
logInform("findSphereTouching3Points() COLINEAR QQQQ");
logInform(" a = (%7.3f %7.3f %7.3f)",
a.x(),
a.y(),
a.z());
logInform(" b = (%7.3f %7.3f %7.3f)",
b.x(),
b.y(),
b.z());
logInform(" c = (%7.3f %7.3f %7.3f)",
c.x(),
c.y(),
c.z());
logInform(" s = (%7.3f %7.3f %7.3f) r=%7.3f",
center.x(),
center.y(),
center.z(),
radius);
}
return;
}
Eigen::Vector3d rel_center = (ac_lensq * n.cross(ab) +
ab_lensq * ac.cross(n)) /
(2.0 * n_lensq);
radius = rel_center.norm();
center = rel_center + a;
if (g_verbose)
{
logInform("findSphereTouching3Points()");
logInform(" a = (%7.3f %7.3f %7.3f)",
a.x(),
a.y(),
a.z());
logInform(" b = (%7.3f %7.3f %7.3f)",
b.x(),
b.y(),
b.z());
logInform(" c = (%7.3f %7.3f %7.3f)",
c.x(),
c.y(),
c.z());
logInform(" s = (%7.3f %7.3f %7.3f) r=%7.3f",
center.x(),
center.y(),
center.z(),
radius);
}
}