/*
* Function: report
* Description: Packet and Send data to userspace by kstate
* Input: mask -- message mask
* fmt -- string
* Return: -1--failed, 0--success
**/
static int report(int mask, va_list args, const char *fmt)
{
int ret = -1;
struct kcollect_info info;
struct timeval time;
int length = 0;
size_t info_len = 0;
memset(&info, 0, sizeof(info));
length = vscnprintf(info.buffer, KCOLLECT_BUFFER_SIZE - 1, fmt, args);
if (length > 0) {
info.mask = mask;
info.len = length + 1;
if (!get_ktime_disable()) {
do_gettimeofday(&time);
}
info.tv_sec = (u32)time.tv_sec;
info.tv_usec = (u32)time.tv_usec;
info_len = sizeof(info) - KCOLLECT_BUFFER_SIZE + length + 1;
ret = kstate(CHANNEL_ID_KCOLLECT, PACKET_TAG_KCOLLECT, (char*)&info, info_len);
if (ret < 0) {
pr_err("hw_kcollect %s: kstate error\n", __func__);
ret = -1;
}
}
pr_debug("hw_kcollect %s: length=%d mask=%d\n", __func__, length, mask);
return ret;
}
TEST_F(DistanceFieldCollisionDetectionTester, LinksInCollision)
{
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res1;
collision_detection::CollisionResult res2;
collision_detection::CollisionResult res3;
//req.contacts = true;
//req.max_contacts = 100;
req.group_name = "whole_body";
planning_models::KinematicState kstate(kmodel_);
kstate.setToDefaultValues();
Eigen::Affine3d offset = Eigen::Affine3d::Identity();
offset.translation().x() = .01;
kstate.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
kstate.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
acm_->setEntry("base_link", "base_bellow_link", false);
crobot_->checkSelfCollision(req, res1, kstate, *acm_);
ASSERT_TRUE(res1.collision);
acm_->setEntry("base_link", "base_bellow_link", true);
crobot_->checkSelfCollision(req, res2, kstate, *acm_);
ASSERT_FALSE(res2.collision);
// req.verbose = true;
kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);
acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
crobot_->checkSelfCollision(req, res3, kstate, *acm_);
ASSERT_TRUE(res3.collision);
}
TEST_F(DistanceFieldCollisionDetectionTester, ContactPositions)
{
collision_detection::CollisionRequest req;
req.contacts = true;
req.max_contacts = 1;
req.group_name = "whole_body";
planning_models::KinematicState kstate(kmodel_);
kstate.setToDefaultValues();
Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
Eigen::Affine3d pos2 = Eigen::Affine3d::Identity();
pos1.translation().x() = 5.0;
pos2.translation().x() = 5.01;
kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
collision_detection::CollisionResult res;
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_TRUE(res.collision);
ASSERT_EQ(res.contacts.size(),1);
ASSERT_EQ(res.contacts.begin()->second.size(),1);
for(collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
it != res.contacts.end();
it++) {
EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33);
}
pos1 = Eigen::Affine3d(Eigen::Translation3d(3.0,0.0,0.0)*Eigen::Quaterniond::Identity());
pos2 = Eigen::Affine3d(Eigen::Translation3d(3.0,0.0,0.0)*Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0));
kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
collision_detection::CollisionResult res2;
crobot_->checkSelfCollision(req, res2, kstate, *acm_);
ASSERT_TRUE(res2.collision);
ASSERT_EQ(res2.contacts.size(),1);
ASSERT_EQ(res2.contacts.begin()->second.size(),1);
for(collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin();
it != res2.contacts.end();
it++) {
ROS_INFO_STREAM("Col x is " << it->second[0].pos.x());
EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33);
}
pos1 = Eigen::Affine3d(Eigen::Translation3d(3.0,0.0,0.0)*Eigen::Quaterniond::Identity());
pos2 = Eigen::Affine3d(Eigen::Translation3d(3.0,0.0,0.0)*Eigen::Quaterniond(M_PI/4.0, 0.0, M_PI/4.0, 0.0));
kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
collision_detection::CollisionResult res3;
crobot_->checkSelfCollision(req, res2, kstate, *acm_);
ASSERT_FALSE(res3.collision);
}
TEST_F(FclCollisionDetectionTester, DefaultNotInCollision)
{
robot_state::RobotState kstate(kmodel_);
kstate.setToDefaultValues();
kstate.update();
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_FALSE(res.collision);
}
TEST_F(DistanceFieldCollisionDetectionTester, DefaultNotInCollision)
{
planning_models::KinematicState kstate(kmodel_);
kstate.setToDefaultValues();
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
req.group_name = "whole_body";
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_FALSE(res.collision);
}
TEST_F(FclCollisionDetectionTester, ContactReporting)
{
collision_detection::CollisionRequest req;
req.contacts = true;
req.max_contacts = 1;
robot_state::RobotState kstate(kmodel_);
kstate.setToDefaultValues();
kstate.update();
Eigen::Affine3d offset = Eigen::Affine3d::Identity();
offset.translation().x() = .01;
// kstate.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
// kstate.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
// kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
// kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);
kstate.updateStateWithLinkAt("base_link", Eigen::Affine3d::Identity());
kstate.updateStateWithLinkAt("base_bellow_link", offset);
kstate.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Affine3d::Identity());
kstate.updateStateWithLinkAt("l_gripper_palm_link", offset);
kstate.update();
acm_->setEntry("base_link", "base_bellow_link", false);
acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
collision_detection::CollisionResult res;
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_TRUE(res.collision);
EXPECT_EQ(res.contacts.size(),1);
EXPECT_EQ(res.contacts.begin()->second.size(),1);
res.clear();
req.max_contacts = 2;
req.max_contacts_per_pair = 1;
// req.verbose = true;
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_TRUE(res.collision);
EXPECT_EQ(res.contacts.size(), 2);
EXPECT_EQ(res.contacts.begin()->second.size(),1);
res.contacts.clear();
res.contact_count = 0;
req.max_contacts = 10;
req.max_contacts_per_pair = 2;
acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), false));
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_TRUE(res.collision);
EXPECT_LE(res.contacts.size(), 10);
EXPECT_LE(res.contact_count, 10);
}
TEST_F(FclCollisionDetectionTester, DiffSceneTester)
{
robot_state::RobotState kstate(kmodel_);
kstate.setToDefaultValues();
kstate.update();
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
collision_detection::CollisionRobotFCL new_crobot(*(dynamic_cast<collision_detection::CollisionRobotFCL*>(crobot_.get())));
ros::WallTime before = ros::WallTime::now();
new_crobot.checkSelfCollision(req,res,kstate);
double first_check = (ros::WallTime::now()-before).toSec();
before = ros::WallTime::now();
new_crobot.checkSelfCollision(req,res,kstate);
double second_check = (ros::WallTime::now()-before).toSec();
EXPECT_LT(fabs(first_check-second_check), .05);
std::vector<shapes::ShapeConstPtr> shapes;
shapes.resize(1);
shapes[0].reset(shapes::createMeshFromResource(kinect_dae_resource_));
EigenSTL::vector_Affine3d poses;
poses.push_back(Eigen::Affine3d::Identity());
std::vector<std::string> touch_links;
kstate.attachBody("kinect", shapes, poses, touch_links, "r_gripper_palm_link");
before = ros::WallTime::now();
new_crobot.checkSelfCollision(req,res,kstate);
first_check = (ros::WallTime::now()-before).toSec();
before = ros::WallTime::now();
new_crobot.checkSelfCollision(req,res,kstate);
second_check = (ros::WallTime::now()-before).toSec();
//the first check is going to take a while, as data must be constructed
EXPECT_LT(second_check, .1);
collision_detection::CollisionRobotFCL other_new_crobot(*(dynamic_cast<collision_detection::CollisionRobotFCL*>(crobot_.get())));
before = ros::WallTime::now();
new_crobot.checkSelfCollision(req,res,kstate);
first_check = (ros::WallTime::now()-before).toSec();
before = ros::WallTime::now();
new_crobot.checkSelfCollision(req,res,kstate);
second_check = (ros::WallTime::now()-before).toSec();
EXPECT_LT(fabs(first_check-second_check), .05);
}
TEST_F(DistanceFieldCollisionDetectionTester, DefaultNotInCollision)
{
robot_state::RobotState kstate(kmodel_);
kstate.setToDefaultValues();
ASSERT_TRUE(urdf_ok_ && srdf_ok_);
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
req.group_name = "whole_body";
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_FALSE(res.collision);
}
TEST_F(DistanceFieldCollisionDetectionTester, ChangeTorsoPosition) {
planning_models::KinematicState kstate(kmodel_);
kstate.setToDefaultValues();
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res1;
collision_detection::CollisionResult res2;
req.group_name = "right_arm";
crobot_->checkSelfCollision(req, res1, kstate, *acm_);
std::map<std::string, double> torso_val;
torso_val["torso_lift_joint"] = .15;
kstate.setStateValues(torso_val);
crobot_->checkSelfCollision(req, res1, kstate, *acm_);
crobot_->checkSelfCollision(req, res1, kstate, *acm_);
}
bool transform(state_transformer::GetTransform::Request &req,
state_transformer::GetTransform::Response &res) {
planning_models::KinematicState kstate(robot_model.getKinematicModel());
if (!planning_environment::setRobotStateAndComputeTransforms
(req.robot_state, kstate)) {
ROS_ERROR("Unable to transform robot state to kinematic state");
res.val = res.BAD_ROBOT_STATE;
return true;
}
std::string from_frame = relative_frame(req.from_frame_id);
std::string to_frame = relative_frame(req.to_frame_id);
ROS_INFO("Transforming from %s to %s", from_frame.c_str(), to_frame.c_str());
std::vector<geometry_msgs::TransformStamped> transforms;
btTransform global_to_to, global_to_from;
if (!world_transform(from_frame, kstate, global_to_from)) {
res.val = res.UNABLE_TO_FIND_FROM_FRAME;
return true;
}
if (!world_transform(to_frame, kstate, global_to_to)) {
res.val = res.UNABLE_TO_FIND_TO_FRAME;
return true;
}
//This is the transform that will take the origin of to
//to the origin of from. This is how TF works so we are sticking
//with that convention although it is confusing. The transform
//with from=robot frame to=wrist frame will give you the position of
//the wrist in the robot frame for example.
//
//HOWEVER, the transform that takes a pose expressed in from and transforms
//it to a pose expressed in to is the INVERSE of this transform
btTransform trans = (global_to_from.inverse())*global_to_to;
tf::transformTFToMsg(trans, res.transform_stamped.transform);
res.transform_stamped.header.stamp = ros::Time(0);
res.transform_stamped.header.frame_id = req.from_frame_id;
res.transform_stamped.child_frame_id = req.to_frame_id;
res.val = res.SUCCESS;
return true;
}
TEST_F(LoadPlanningModelsPr2, StateSpaceCopy)
{
ompl_interface::ModelBasedStateSpaceSpecification spec(kmodel_, "right_arm");
ompl_interface::JointModelStateSpace ss(spec);
ss.setPlanningVolume(-1, 1, -1, 1, -1, 1);
ss.setup();
std::ofstream fout("ompl_interface_test_state_space_diagram1.dot");
ss.diagram(fout);
bool passed = false;
try
{
ss.sanityChecks();
passed = true;
}
catch(ompl::Exception &ex)
{
logError("Sanity checks did not pass: %s", ex.what());
}
EXPECT_TRUE(passed);
robot_state::RobotState kstate(kmodel_);
kstate.setToRandomValues();
EXPECT_TRUE(kstate.distance(kstate) < 1e-12);
ompl::base::State *state = ss.allocState();
for (int i = 0 ; i < 10 ; ++i)
{
robot_state::RobotState kstate2(kstate);
EXPECT_TRUE(kstate.distance(kstate2) < 1e-12);
ss.copyToOMPLState(state, kstate);
kstate.getJointStateGroup(ss.getJointModelGroupName())->setToRandomValues();
std::cout << (kstate.getLinkState("r_wrist_roll_link")->getGlobalLinkTransform().translation() -
kstate2.getLinkState("r_wrist_roll_link")->getGlobalLinkTransform().translation()) << std::endl;
EXPECT_TRUE(kstate.distance(kstate2) > 1e-12);
ss.copyToRobotState(kstate, state);
std::cout << (kstate.getLinkState("r_wrist_roll_link")->getGlobalLinkTransform().translation() -
kstate2.getLinkState("r_wrist_roll_link")->getGlobalLinkTransform().translation()) << std::endl;
EXPECT_TRUE(kstate.distance(kstate2) < 1e-12);
}
ss.freeState(state);
}
TEST_F(FclCollisionDetectionTester, LinksInCollision)
{
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res1;
collision_detection::CollisionResult res2;
collision_detection::CollisionResult res3;
//req.contacts = true;
//req.max_contacts = 100;
robot_state::RobotState kstate(kmodel_);
kstate.setToDefaultValues();
kstate.update();
Eigen::Affine3d offset = Eigen::Affine3d::Identity();
offset.translation().x() = .01;
// kstate.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
// kstate.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
kstate.updateStateWithLinkAt("base_link", Eigen::Affine3d::Identity());
kstate.updateStateWithLinkAt("base_bellow_link", offset);
kstate.update();
acm_->setEntry("base_link", "base_bellow_link", false);
crobot_->checkSelfCollision(req, res1, kstate, *acm_);
ASSERT_TRUE(res1.collision);
acm_->setEntry("base_link", "base_bellow_link", true);
crobot_->checkSelfCollision(req, res2, kstate, *acm_);
ASSERT_FALSE(res2.collision);
// req.verbose = true;
// kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
// kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);
kstate.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Affine3d::Identity());
kstate.updateStateWithLinkAt("l_gripper_palm_link", offset);
kstate.update();
acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
crobot_->checkSelfCollision(req, res3, kstate, *acm_);
ASSERT_TRUE(res3.collision);
}
// ******************************************************************************************
// Thread for getting the pairs of links that are never in collision
// ******************************************************************************************
void disableNeverInCollisionThread(ThreadComputation tc)
{
//ROS_INFO_STREAM("Thread " << tc.thread_id_ << " running " << tc.num_trials_ << " trials");
// User feedback vars
const unsigned int progress_interval = tc.num_trials_ / 20; // show progress update every 5%
// Create a new kinematic state for this thread to work on
robot_state::RobotState kstate(tc.scene_.getRobotModel());
// Do a large number of tests
for (unsigned int i = 0 ; i < tc.num_trials_ ; ++i)
{
// Status update at intervals and only for 0 thread
if(i % progress_interval == 0 && tc.thread_id_ == 0)
{
//ROS_INFO("Collision checking %d%% complete", int(i * 100 / tc.num_trials_ ));
(*tc.progress_) = i * 92 / tc.num_trials_ + 8; // 8 is the amount of progress already completed in prev steps
}
collision_detection::CollisionResult res;
kstate.setToRandomPositions();
tc.scene_.checkSelfCollision(tc.req_, res, kstate);
// Check all contacts
for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin() ; it != res.contacts.end() ; ++it)
{
// Check if this collision pair is unique before doing a thread lock
if (tc.links_seen_colliding_->find( it->first ) == tc.links_seen_colliding_->end())
{
// Collision Matrix and links_seen_colliding is modified only if needed, based on above if statement
boost::mutex::scoped_lock slock(*tc.lock_);
tc.links_seen_colliding_->insert(it->first);
tc.scene_.getAllowedCollisionMatrixNonConst().setEntry(it->first.first, it->first.second, true); // disable link checking in the collision matrix
}
}
}
}
void ompl_interface::ConstraintApproximation::visualizeDistribution(const std::string &link_name, unsigned int count, visualization_msgs::MarkerArray &arr) const
{
robot_state::RobotState kstate(kmodel_);
kstate.setToDefaultValues();
ompl::RNG rng;
std_msgs::ColorRGBA color;
color.r = 0.0f;
color.g = 1.0f;
color.b = 1.0f;
color.a = 1.0f;
if (state_storage_->size() < count)
count = state_storage_->size();
for (std::size_t i = 0 ; i < count ; ++i)
{
state_storage_->getStateSpace()->as<ModelBasedStateSpace>()->copyToRobotState(kstate, state_storage_->getState(rng.uniformInt(0, state_storage_->size() - 1)));
const Eigen::Vector3d &pos = kstate.getLinkState(link_name)->getGlobalLinkTransform().translation();
visualization_msgs::Marker mk;
mk.header.stamp = ros::Time::now();
mk.header.frame_id = kmodel_->getModelFrame();
mk.ns = "stored_constraint_data";
mk.id = i;
mk.type = visualization_msgs::Marker::SPHERE;
mk.action = visualization_msgs::Marker::ADD;
mk.pose.position.x = pos.x();
mk.pose.position.y = pos.y();
mk.pose.position.z = pos.z();
mk.pose.orientation.w = 1.0;
mk.scale.x = mk.scale.y = mk.scale.z = 0.035;
mk.color = color;
mk.lifetime = ros::Duration(30.0);
arr.markers.push_back(mk);
}
}
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;
}
TEST_F(FclCollisionDetectionTester, AttachedBodyTester) {
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));
robot_state::RobotState kstate(kmodel_);
kstate.setToDefaultValues();
kstate.update();
Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
pos1.translation().x() = 5.0;
// kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
kstate.update();
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_FALSE(res.collision);
shapes::Shape* shape = new shapes::Box(.1,.1,.1);
cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
res = collision_detection::CollisionResult();
cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
ASSERT_TRUE(res.collision);
//deletes shape
cworld_->getWorld()->removeObject("box");
shape = new shapes::Box(.1,.1,.1);
std::vector<shapes::ShapeConstPtr> shapes;
EigenSTL::vector_Affine3d poses;
shapes.push_back(shapes::ShapeConstPtr(shape));
poses.push_back(Eigen::Affine3d::Identity());
std::vector<std::string> touch_links;
kstate.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
res = collision_detection::CollisionResult();
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_TRUE(res.collision);
//deletes shape
kstate.clearAttachedBody("box");
touch_links.push_back("r_gripper_palm_link");
touch_links.push_back("r_gripper_motor_accelerometer_link");
shapes[0].reset(new shapes::Box(.1,.1,.1));
kstate.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
kstate.update();
res = collision_detection::CollisionResult();
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_FALSE(res.collision);
pos1.translation().x() = 5.01;
shapes::Shape* coll = new shapes::Box(.1, .1, .1);
cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
res = collision_detection::CollisionResult();
cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
ASSERT_TRUE(res.collision);
acm_->setEntry("coll", "r_gripper_palm_link", true);
res = collision_detection::CollisionResult();
cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
ASSERT_TRUE(res.collision);
}
TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester) {
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
req.group_name = "right_arm";
acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));
planning_models::KinematicState kstate(kmodel_);
kstate.setToDefaultValues();
Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
pos1.translation().x() = 1.0;
kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_FALSE(res.collision);
shapes::Shape* shape = new shapes::Box(.25,.25,.25);
cworld_->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
res = collision_detection::CollisionResult();
cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
ASSERT_TRUE(res.collision);
//deletes shape
cworld_->removeObject("box");
shape = new shapes::Box(.25,.25,.25);
std::vector<shapes::ShapeConstPtr> shapes;
std::vector<Eigen::Affine3d> poses;
shapes.push_back(shapes::ShapeConstPtr(shape));
poses.push_back(Eigen::Affine3d::Identity());
std::vector<std::string> touch_links;
kstate.getLinkState("r_gripper_palm_link")->attachBody("box", shapes, poses, touch_links);
res = collision_detection::CollisionResult();
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_TRUE(res.collision);
//deletes shape
kstate.getLinkState("r_gripper_palm_link")->clearAttachedBody("box");
touch_links.push_back("r_gripper_palm_link");
shapes[0].reset(new shapes::Box(.1,.1,.1));
kstate.getLinkState("r_gripper_palm_link")->attachBody("box", shapes, poses, touch_links);
res = collision_detection::CollisionResult();
crobot_->checkSelfCollision(req, res, kstate, *acm_);
ASSERT_FALSE(res.collision);
pos1.translation().x() = 1.01;
shapes::Shape* coll = new shapes::Box(.1, .1, .1);
cworld_->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
res = collision_detection::CollisionResult();
cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
ASSERT_TRUE(res.collision);
acm_->setEntry("coll", "r_gripper_palm_link", true);
res = collision_detection::CollisionResult();
cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
ASSERT_TRUE(res.collision);
}
TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached)
{
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
shapes::ShapeConstPtr shape(shapes::createMeshFromResource(kinect_dae_resource_));
Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
Eigen::Affine3d pos2 = Eigen::Affine3d::Identity();
pos2.translation().x() = 10.0;
cworld_->getWorld()->addToObject("kinect", shape, pos1);
robot_state::RobotState kstate(kmodel_);
kstate.setToDefaultValues();
kstate.update();
ros::WallTime before = ros::WallTime::now();
cworld_->checkRobotCollision(req, res, *crobot_, kstate);
double first_check = (ros::WallTime::now()-before).toSec();
before = ros::WallTime::now();
cworld_->checkRobotCollision(req, res, *crobot_, kstate);
double second_check = (ros::WallTime::now()-before).toSec();
EXPECT_LT(second_check, .05);
collision_detection::CollisionWorld::ObjectPtr object = cworld_->getWorld()->getObject("kinect");
cworld_->getWorld()->removeObject("kinect");
robot_state::RobotState kstate1(kmodel_);
robot_state::RobotState kstate2(kmodel_);
kstate1.setToDefaultValues();
kstate2.setToDefaultValues();
kstate1.update();
kstate2.update();
std::vector<std::string> touch_links;
kstate1.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link");
EigenSTL::vector_Affine3d other_poses;
other_poses.push_back(pos2);
// This creates a new set of constant properties for the attached body, which happens to be the same as the one above;
kstate2.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link");
//going to take a while, but that's fine
res = collision_detection::CollisionResult();
crobot_->checkSelfCollision(req,res,kstate1);
EXPECT_TRUE(res.collision);
before = ros::WallTime::now();
crobot_->checkSelfCollision(req,res,kstate1, *acm_);
first_check = (ros::WallTime::now()-before).toSec();
before = ros::WallTime::now();
req.verbose = true;
res = collision_detection::CollisionResult();
crobot_->checkSelfCollision(req,res,kstate2, *acm_);
second_check = (ros::WallTime::now()-before).toSec();
EXPECT_LT(first_check, .05);
EXPECT_LT(fabs(first_check-second_check), .1);
}
static int lowmem_shrink(struct shrinker *s, struct shrink_control *sc)
{
struct task_struct *tsk;
struct task_struct *selected = NULL;
int rem = 0;
int tasksize;
int i;
short min_score_adj = OOM_SCORE_ADJ_MAX + 1;
int minfree = 0;
int selected_tasksize = 0;
short selected_oom_score_adj;
int array_size = ARRAY_SIZE(lowmem_adj);
int other_free = global_page_state(NR_FREE_PAGES) - totalreserve_pages;
int other_file = global_page_state(NR_FILE_PAGES) -
global_page_state(NR_SHMEM);
if (lowmem_adj_size < array_size)
array_size = lowmem_adj_size;
if (lowmem_minfree_size < array_size)
array_size = lowmem_minfree_size;
for (i = 0; i < array_size; i++) {
minfree = lowmem_minfree[i];
if (other_free < minfree && other_file < minfree) {
min_score_adj = lowmem_adj[i];
break;
}
}
if (sc->nr_to_scan > 0)
lowmem_print(3, "lowmem_shrink %lu, %x, ofree %d %d, ma %hd\n",
sc->nr_to_scan, sc->gfp_mask, other_free,
other_file, min_score_adj);
rem = global_page_state(NR_ACTIVE_ANON) +
global_page_state(NR_ACTIVE_FILE) +
global_page_state(NR_INACTIVE_ANON) +
global_page_state(NR_INACTIVE_FILE);
if (sc->nr_to_scan <= 0 || min_score_adj == OOM_SCORE_ADJ_MAX + 1) {
lowmem_print(5, "lowmem_shrink %lu, %x, return %d\n",
sc->nr_to_scan, sc->gfp_mask, rem);
return rem;
}
selected_oom_score_adj = min_score_adj;
rcu_read_lock();
for_each_process(tsk) {
struct task_struct *p;
short oom_score_adj;
if (tsk->flags & PF_KTHREAD)
continue;
p = find_lock_task_mm(tsk);
if (!p)
continue;
if (test_tsk_thread_flag(p, TIF_MEMDIE) &&
time_before_eq(jiffies, lowmem_deathpending_timeout)) {
task_unlock(p);
rcu_read_unlock();
return 0;
}
oom_score_adj = p->signal->oom_score_adj;
if (oom_score_adj < min_score_adj) {
task_unlock(p);
continue;
}
tasksize = get_mm_rss(p->mm);
task_unlock(p);
if (tasksize <= 0)
continue;
if (selected) {
if (oom_score_adj < selected_oom_score_adj)
continue;
if (oom_score_adj == selected_oom_score_adj &&
tasksize <= selected_tasksize)
continue;
}
selected = p;
selected_tasksize = tasksize;
selected_oom_score_adj = oom_score_adj;
lowmem_print(2, "select '%s' (%d), adj %hd, size %d, to kill\n",
p->comm, p->pid, oom_score_adj, tasksize);
}
if (selected) {
lowmem_print(1, "Killing '%s' (%d), adj %hd,\n" \
" to free %ldkB on behalf of '%s' (%d) because\n" \
" cache %ldkB is below limit %ldkB for oom_score_adj %hd\n" \
" Free memory is %ldkB above reserved\n",
selected->comm, selected->pid,
selected_oom_score_adj,
selected_tasksize * (long)(PAGE_SIZE / 1024),
current->comm, current->pid,
other_file * (long)(PAGE_SIZE / 1024),
minfree * (long)(PAGE_SIZE / 1024),
min_score_adj,
other_free * (long)(PAGE_SIZE / 1024));
lowmem_deathpending_timeout = jiffies + HZ;
#ifdef CONFIG_HUAWEI_KSTATE
kstate(KSTATE_FREEZER_MASK, "[PID %d KILLED][SIG %d]", selected->tgid, SIGKILL);
#endif
send_sig(SIGKILL, selected, 0);
set_tsk_thread_flag(selected, TIF_MEMDIE);
rem -= selected_tasksize;
}
lowmem_print(4, "lowmem_shrink %lu, %x, return %d\n",
sc->nr_to_scan, sc->gfp_mask, rem);
rcu_read_unlock();
return rem;
}