From a83c8645c347b0cc4f7fecdbdf420d7cf2c7e65c Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 13 Dec 2023 09:38:21 -0700 Subject: [PATCH] remove commented out code --- .../src/chomp_optimizer.cpp | 126 ------------------ .../src/detail/constraints_library.cpp | 42 ------ .../work_space/pose_model_state_space.cpp | 4 - .../src/compute_default_collisions.cpp | 26 ---- 4 files changed, 198 deletions(-) diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index db4a1cd0a1..4e8a4d9e5a 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -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 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) @@ -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++; @@ -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) { @@ -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& 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(jointState->getJointModel()); -// if(revolute_joint && revolute_joint->continuous_) { -// continuous = true; -// } - -// map > bounds = jointState->getJointModel()->getAllVariableBounds(); -// int j = 0; -// for(map >::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 diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index 87cb2c31b1..20c1d97d7a 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -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()->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) { diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index d6e48e5cfe..58bb0468c6 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp @@ -359,10 +359,6 @@ void PoseModelStateSpace::copyToOMPLState(ompl::base::State* state, const moveit state->as()->setJointsComputed(true); state->as()->setPoseComputed(false); computeStateFK(state); - /* - std::cout << "COPY STATE IN:\n"; - printState(state, std::cout); - std::cout << "---------- COPY STATE IN\n"; */ } } // namespace ompl_interface diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp index d13a419390..7e72724d84 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp @@ -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 @@ -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: @@ -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 @@ -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())); } // ****************************************************************************************** @@ -433,9 +422,6 @@ unsigned int disableAdjacentLinks(planning_scene::PlanningScene& scene, LinkGrap for (std::set::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 { @@ -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; } @@ -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; } @@ -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)); } } @@ -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; @@ -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) { @@ -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; } @@ -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%