Skip to content

Commit

Permalink
remove commented out code
Browse files Browse the repository at this point in the history
  • Loading branch information
@tylerjw
tylerjw committed Dec 13, 2023
1 parent 5d520f3 commit a83c864
Show file tree
Hide file tree
Showing 4 changed files with 0 additions and 198 deletions.
126 changes: 0 additions & 126 deletions moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -547,40 +547,6 @@ bool ChompOptimizer::isCurrentTrajectoryMeshToMeshCollisionFree() const
return planning_scene_->isPathValid(start_state_msg, traj, planning_group_);
}

/// TODO: HMC BASED COMMENTED CODE BELOW, Need to uncomment and perform extensive testing by varying the HMC parameters
/// values in the chomp_planning.yaml file so that CHOMP can find optimal paths

// CollisionProximitySpace::TrajectorySafety ChompOptimizer::checkCurrentIterValidity()
// {
// JointTrajectory jointTrajectory;
// jointTrajectory.joint_names = joint_names_;
// jointTrajectory.header.frame_id = collision_space_->getCollisionModelsInterface()->getRobotFrameId();
// jointTrajectory.header.stamp = ros::Time::now();
// Constraints goalConstraints;
// Constraints pathConstraints;
// ArmNavigationErrorCodes errorCode;
// vector<ArmNavigationErrorCodes> trajectoryErrorCodes;
// for(int i = 0; i < group_trajectory_.getNumPoints(); ++i)
// {
// JointTrajectoryPoint point;
// for(int j = 0; j < group_trajectory_.getNumJoints(); ++j)
// {
// point.positions.push_back(best_group_trajectory_(i, j));
// }
// jointTrajectory.points.push_back(point);
// }

// return collision_space_->isTrajectorySafe(jointTrajectory, goalConstraints, pathConstraints, planning_group_);
// /*
// bool valid = collision_space_->getCollisionModelsInterface()->isJointTrajectoryValid(*state_,
// jointTrajectory,
// goalConstraints,
// pathConstraints, errorCode,
// trajectoryErrorCodes, false);
// */

// }

void ChompOptimizer::calculateSmoothnessIncrements()
{
for (int i = 0; i < num_joints_; ++i)
Expand Down Expand Up @@ -959,16 +925,6 @@ void ChompOptimizer::performForwardKinematics()
if (point_is_in_collision_[i][j])
{
state_is_in_collision_[i] = true;
// if(is_collision_free_ == true) {
// RCLCPP_INFO(getLogger(),"We know it's not collision free " << g);
// RCLCPP_INFO(getLogger(),"Sphere location " << info.sphere_locations[k].x() << " " <<
// info.sphere_locations[k].y() << " " << info.sphere_locations[k].z());
// RCLCPP_INFO(getLogger(),"Gradient " << info.gradients[k].x() << " " << info.gradients[k].y() << " " <<
// info.gradients[k].z() << " distance " << info.distances[k] << " radii " << info.sphere_radii[k]);
// RCLCPP_INFO(getLogger(),"Radius " << info.sphere_radii[k] << " potential " <<
// collision_point_potential_[i][j]);
// }

is_collision_free_ = false;
}
j++;
Expand All @@ -977,8 +933,6 @@ void ChompOptimizer::performForwardKinematics()
}
}

// RCLCPP_INFO(getLogger(),"Total dur " << total_dur << " total checks " << end-start+1);

// now, get the vel and acc for each collision point (using finite differencing)
for (int i = free_vars_start_; i <= free_vars_end_; ++i)
{
Expand Down Expand Up @@ -1046,84 +1000,4 @@ void ChompOptimizer::perturbTrajectory()
}
}

/// TODO: HMC BASED COMMENTED CODE BELOW, Need to uncomment and perform extensive testing by varying the HMC parameters
/// values in the chomp_planning.yaml file so that CHOMP can find optimal paths
// void ChompOptimizer::getRandomState(const RobotState currentState, const string& groupName, Eigen::VectorXd&
// state_vec)
// {
// const vector<RobotState *::JointState*>& jointStates =
// currentState->getJointStateGroup(groupName)->getJointStateVector();
// for(size_t i = 0; i < jointStates.size(); ++i)
// {

// bool continuous = false;

// RobotState *::JointState* jointState = jointStates[i];
// const RevoluteJointModel* revolute_joint
// = dynamic_cast<const RevoluteJointModel*>(jointState->getJointModel());
// if(revolute_joint && revolute_joint->continuous_) {
// continuous = true;
// }

// map<string, pair<double, double> > bounds = jointState->getJointModel()->getAllVariableBounds();
// int j = 0;
// for(map<string, pair<double, double> >::iterator it = bounds.begin(); it != bounds.end(); ++it)
// {
// double randVal = jointState->getJointStateValues()[j] + (rsl::uniform_real(0., 1.)
// * (parameters_->getRandomJumpAmount()) -
// rsl::uniform_real(0., 1.) *
// (parameters_->getRandomJumpAmount()));

// if(!continuous)
// {
// if(randVal > it->second.second)
// {
// randVal = it->second.second;
// }
// else if(randVal < it->second.first)
// {
// randVal = it->second.first;
// }
// }

// ROS_DEBUG("Joint " << it->first << " old value " << jointState->getJointStateValues()[j] << " new value
// " << randVal);
// state_vec(i) = randVal;

// j++;
// }
// }
// }

/*
void ChompOptimizer::getRandomMomentum()
{
if (is_collision_free_)
random_momentum_.setZero(num_vars_free_, num_joints_);
else
for (int i = 0; i < num_joints_; ++i)
{
multivariate_gaussian_[i].sample(random_joint_momentum_);
random_momentum_.col(i) = stochasticity_factor_ * random_joint_momentum_;
}
}
void ChompOptimizer::updateMomentum()
{
// double alpha = 1.0 - parameters_->getHmcStochasticity();
double eps = parameters_->getHmcDiscretization();
if (iteration_ > 0)
momentum_ = (momentum_ + eps * final_increments_);
else
momentum_ = random_momentum_;
// momentum_ = alpha * (momentum_ + eps*final_increments_) + sqrt(1.0-alpha*alpha)*random_momentum_;
}
void ChompOptimizer::updatePositionFromMomentum()
{
double eps = parameters_->getHmcDiscretization();
group_trajectory_.getFreeTrajectoryBlock() += eps * momentum_;
}
*/

} // namespace chomp
42 changes: 0 additions & 42 deletions moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -273,48 +273,6 @@ ConstraintApproximation::getStateSamplerAllocator(const moveit_msgs::msg::Constr
return allocConstraintApproximationStateSampler(ss, space_signature_, state_storage_, milestones_);
};
}
/*
void ConstraintApproximation::visualizeDistribution(const
std::string &link_name, unsigned int count,
visualization_msgs::MarkerArray &arr) const
{
moveit::core::RobotState robot_state(robot_model_);
robot_state.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(robot_state,
state_storage_->getState(rng.uniformInt(0, state_storage_->size() - 1)));
const Eigen::Vector3d &pos =
robot_state.getLinkState(link_name)->getGlobalLinkTransform().translation();
visualization_msgs::Marker mk;
mk.header.stamp = ros::Time::now();
mk.header.frame_id = robot_model_->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);
}
}
*/

void ConstraintsLibrary::loadConstraintApproximations(const std::string& path)
{
Expand Down
4 changes: 0 additions & 4 deletions ...t_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -359,10 +359,6 @@ void PoseModelStateSpace::copyToOMPLState(ompl::base::State* state, const moveit
state->as<StateType>()->setJointsComputed(true);
state->as<StateType>()->setPoseComputed(false);
computeStateFK(state);
/*
std::cout << "COPY STATE IN:\n";
printState(state, std::cout);
std::cout << "---------- COPY STATE IN\n"; */
}

} // namespace ompl_interface
26 changes: 0 additions & 26 deletions moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -194,8 +194,6 @@ LinkPairMap computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr
// LinkGraph is a custom type of a map with a LinkModel as key and a set of LinkModels as second
LinkGraph link_graph;

// RCLCPP_INFO_STREAM_STREAM(getLogger(), "Initial allowed Collision Matrix Size = " << scene.getAllowedCollisions().getSize() );

// 0. GENERATE ALL POSSIBLE LINK PAIRS -------------------------------------------------------------
// Generate a list of unique link pairs for all links with geometry. Order pairs alphabetically.
// There should be n choose 2 pairs
Expand Down Expand Up @@ -248,8 +246,6 @@ LinkPairMap computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr
num_never = disableNeverInCollision(num_trials, *scene, link_pairs, req, links_seen_colliding, progress);
}

// RCLCPP_INFO_STREAM(getLogger(), "Link pairs seen colliding ever: %d", int(links_seen_colliding.size()));

if (verbose)
{
// Calculate number of disabled links:
Expand All @@ -274,12 +270,6 @@ LinkPairMap computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr
RCLCPP_INFO_STREAM(getLogger(), "Adjacent links disabled : " + std::to_string(num_adjacent));
RCLCPP_INFO_STREAM(getLogger(), "Sometimes in collision : " + std::to_string(num_sometimes));
RCLCPP_INFO_STREAM(getLogger(), "TOTAL DISABLED : " + std::to_string(num_disabled));

/*ROS_INFO("Copy to Spreadsheet:");
ROS_INFO_STREAM(num_links << "\t" << num_possible << "\t" << num_always << "\t" << num_never
<< "\t" << num_default << "\t" << num_adjacent << "\t" << num_sometimes
<< "\t" << num_disabled);
*/
}

*progress = 100; // end the status bar
Expand Down Expand Up @@ -392,7 +382,6 @@ void computeConnectionGraph(const moveit::core::LinkModel* start_link, LinkGraph
}
}
}
// RCLCPP_INFO_STREAM(getLogger(), "Generated connection graph with %d links", int(link_graph.size()));
}

// ******************************************************************************************
Expand Down Expand Up @@ -433,9 +422,6 @@ unsigned int disableAdjacentLinks(planning_scene::PlanningScene& scene, LinkGrap
for (std::set<const moveit::core::LinkModel*>::const_iterator adj_it = link_graph_it->second.begin();
adj_it != link_graph_it->second.end(); ++adj_it)
{
// RCLCPP_INFO_STREAM(getLogger(), "Disabled %s to %s", link_graph_it->first->getName().c_str(),
// (*adj_it)->getName().c_str() );

// Check if either of the links have no geometry. If so, do not add (are we sure?)
if (!link_graph_it->first->getShapes().empty() && !(*adj_it)->getShapes().empty()) // both links have geometry
{
Expand All @@ -446,7 +432,6 @@ unsigned int disableAdjacentLinks(planning_scene::PlanningScene& scene, LinkGrap
}
}
}
// RCLCPP_INFO_STREAM(getLogger(), "Disabled %d adjacent link pairs from collision checking", num_disabled);

return num_disabled;
}
Expand Down Expand Up @@ -474,9 +459,6 @@ unsigned int disableDefaultCollisions(planning_scene::PlanningScene& scene, Link
scene.getAllowedCollisionMatrixNonConst().setEntry(it->first.first, it->first.second, true);
}

// RCLCPP_INFO_STREAM(getLogger(), "Disabled %d link pairs that are in collision in default state from collision
// checking", num_disabled);

return num_disabled;
}

Expand Down Expand Up @@ -522,7 +504,6 @@ unsigned int disableAlwaysInCollision(planning_scene::PlanningScene& scene, Link
if (nc >= req.max_contacts)
{
req.max_contacts *= 2; // double the max contacts that the CollisionRequest checks for
// RCLCPP_INFO_STREAM(getLogger(), "Doubling max_contacts to %d", int(req.max_contacts));
}
}

Expand All @@ -549,8 +530,6 @@ unsigned int disableAlwaysInCollision(planning_scene::PlanningScene& scene, Link
// if no updates were made to the collision matrix, we stop
if (found == 0)
done = true;

// RCLCPP_INFO_STREAM(getLogger(), "Disabled %u link pairs that are always in collision from collision checking", found);
}

return num_disabled;
Expand All @@ -568,8 +547,6 @@ unsigned int disableNeverInCollision(const unsigned int num_trials, planning_sce
std::mutex lock; // used for sharing the same data structures

int num_threads = std::thread::hardware_concurrency(); // how many cores does this computer have?
// RCLCPP_INFO_STREAM_STREAM(getLogger(), "Performing " << num_trials << " trials for 'always in collision' checking on " <<
// num_threads << " threads...");

for (int i = 0; i < num_threads; ++i)
{
Expand Down Expand Up @@ -600,7 +577,6 @@ unsigned int disableNeverInCollision(const unsigned int num_trials, planning_sce
}
}
}
// RCLCPP_INFO_STREAM(getLogger(), "Disabled %d link pairs that are never in collision", num_disabled);

return num_disabled;
}
Expand All @@ -610,8 +586,6 @@ unsigned int disableNeverInCollision(const unsigned int num_trials, planning_sce
// ******************************************************************************************
void disableNeverInCollisionThread(ThreadComputation tc)
{
// RCLCPP_INFO_STREAM_STREAM(getLogger(), "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%

Expand Down

0 comments on commit a83c864

Please sign in to comment.