Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixing bugs #23

Merged
merged 6 commits into from
Aug 11, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion data/xarm6/scenario_real_time/scenario_real_time.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ environment:

random_obstacles:
init_num: 0 # Number of random obstacles to start with the testing
max_num: 100 # Maximal number of random obstacles to be added
max_num: 50 # Maximal number of random obstacles to be added
max_vel: 1.6 # Maximal velocity of each obstacle in [m/s]
max_acc: 0 # Maximal acceleration of each obstacle in [m/s²]
dim: [0.01, 0.01, 0.01] # Dimensions of each random obstacle in [m]
Expand Down
1 change: 0 additions & 1 deletion include/planners/drbt/DRGBT.h
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,6 @@ namespace planning::drbt
std::vector<std::shared_ptr<base::State>> predefined_path; // The predefined path that is being followed
size_t num_lateral_states; // Number of lateral states
float max_edge_length; // Maximal edge length when acquiring a new predefined path
float max_time_generateGBur; // Maximal computing time in [s] for generateGBur routine
bool all_robot_vel_same; // Whether all joint velocities are the same
std::shared_ptr<planning::drbt::Splines> splines; // Everything related to splines
};
Expand Down
2 changes: 1 addition & 1 deletion include/robots/AbstractRobot.h
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ namespace robots
inline size_t getGroundIncluded() const { return ground_included; }

inline void setConfiguration(const std::shared_ptr<base::State> configuration_) { configuration = configuration_; }
inline void setCapsulesRadius(const std::vector<float> &capsules_radius_) { capsules_radius = capsules_radius_; }
inline virtual void setCapsulesRadius(const std::vector<float> &capsules_radius_) { capsules_radius = capsules_radius_; }
inline void setMaxVel(const Eigen::VectorXf &max_vel_) { max_vel = max_vel_; }
inline void setMaxAcc(const Eigen::VectorXf &max_acc_) { max_acc = max_acc_; }
inline void setMaxJerk(const Eigen::VectorXf &max_jerk_) { max_jerk = max_jerk_; }
Expand Down
2 changes: 2 additions & 0 deletions include/robots/xArm6.h
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ namespace robots
const KDL::Tree &getRobotTree() const { return robot_tree; }

void setState(std::shared_ptr<base::State> q) override;
void setCapsulesRadius(const std::vector<float> &capsules_radius_) override;
std::shared_ptr<std::vector<KDL::Frame>> computeForwardKinematics(std::shared_ptr<base::State> q) override;
std::shared_ptr<base::State> computeInverseKinematics(const KDL::Rotation &R, const KDL::Vector &p,
std::shared_ptr<base::State> q_init = nullptr) override;
Expand All @@ -51,6 +52,7 @@ namespace robots
std::vector<KDL::Frame> init_poses;
KDL::Tree robot_tree;
KDL::Chain robot_chain;
std::vector<float> capsules_radius_new;
};
}

Expand Down
17 changes: 12 additions & 5 deletions src/planners/drbt/DRGBT.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,9 +29,6 @@ planning::drbt::DRGBT::DRGBT(const std::shared_ptr<base::StateSpace> ss_, const
planner_info->setNumIterations(0);
path.emplace_back(q_start); // State 'q_start' is added to the realized path
max_edge_length = ss->robot->getMaxVel().norm() * DRGBTConfig::MAX_ITER_TIME;
max_time_generateGBur = DRGBTConfig::MAX_TIME_TASK1;
if (DRGBTConfig::TRAJECTORY_INTERPOLATION == planning::TrajectoryInterpolation::Spline)
max_time_generateGBur -= DRGBTConfig::GUARANTEED_SAFE_MOTION ? SplinesConfig::MAX_TIME_COMPUTE_SAFE : SplinesConfig::MAX_TIME_COMPUTE_REGULAR;

all_robot_vel_same = true;
for (size_t i = 1; i < ss->num_dimensions; i++)
Expand Down Expand Up @@ -81,6 +78,13 @@ bool planning::drbt::DRGBT::solve()
ss->computeDistance(q_current, true); // ~ 1 [ms]
planner_info->addRoutineTime(getElapsedTime(time_computeDistance, planning::TimeUnit::us), 1);
// std::cout << "d_c: " << q_current->getDistance() << " [m] \n";
if (q_current->getDistance() <= 0)
{
std::cout << "*************** Collision has been occurred!!! *************** \n";
planner_info->setSuccessState(false);
planner_info->setPlanningTime(planner_info->getIterationTimes().back());
return false;
}

// ------------------------------------------------------------------------------- //
if (status != base::State::Status::Advanced)
Expand Down Expand Up @@ -247,6 +251,9 @@ void planning::drbt::DRGBT::generateGBur()
auto time_generateGBur { std::chrono::steady_clock::now() };
size_t max_num_attempts {};
float time_elapsed {};
float max_time { DRGBTConfig::MAX_TIME_TASK1 };
if (DRGBTConfig::TRAJECTORY_INTERPOLATION == planning::TrajectoryInterpolation::Spline)
max_time -= DRGBTConfig::GUARANTEED_SAFE_MOTION ? SplinesConfig::MAX_TIME_COMPUTE_SAFE : SplinesConfig::MAX_TIME_COMPUTE_REGULAR;
planner_info->setTask1Interrupted(false);

for (size_t idx = 0; idx < horizon.size(); idx++)
Expand All @@ -258,7 +265,7 @@ void planning::drbt::DRGBT::generateGBur()
{
// Check whether the elapsed time for Task 1 is exceeded
time_elapsed = getElapsedTime(time_iter_start);
if (time_elapsed >= max_time_generateGBur && idx < horizon.size() - 1)
if (time_elapsed >= max_time && idx < horizon.size() - 1)
{
// Delete horizon states for which there is no enough remaining time to be processed
// This is OK since better states are usually located at the beginning of horizon
Expand All @@ -275,7 +282,7 @@ void planning::drbt::DRGBT::generateGBur()
planner_info->addRoutineTime(getElapsedTime(time_generateGBur, planning::TimeUnit::ms), 2);
return;
}
max_num_attempts = std::ceil((1 - time_elapsed / max_time_generateGBur) * DRGBTConfig::MAX_NUM_MODIFY_ATTEMPTS);
max_num_attempts = std::ceil((1 - time_elapsed / max_time) * DRGBTConfig::MAX_NUM_MODIFY_ATTEMPTS);
}
else
max_num_attempts = DRGBTConfig::MAX_NUM_MODIFY_ATTEMPTS;
Expand Down
2 changes: 1 addition & 1 deletion src/planners/drbt/Splines.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -310,7 +310,7 @@ float planning::drbt::Splines::computeDistanceUnderestimation(const std::shared_
(skeleton->col(i+1) - O).dot((R - O).normalized()))
- ss->robot->getCapsuleRadius(i);
if (d_c_temp < 0)
return d_c_temp;
return 0;

d_c = std::min(d_c, d_c_temp);

Expand Down
2 changes: 1 addition & 1 deletion src/planners/rbt/RBTConnect.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,7 @@ std::tuple<base::State::Status, std::shared_ptr<base::State>> planning::rbt::RBT

self_collision = ss->robot->checkSelfCollision(q_temp, q_new);
if (self_collision)
status = !ss->isEqual(q, q_new) ? base::State::Status::Advanced : base::State::Status::Trapped;
status = !ss->isEqual(q_temp, q_new) ? base::State::Status::Advanced : base::State::Status::Trapped;

if (++counter == RBTConnectConfig::NUM_ITER_SPINE || status != base::State::Status::Advanced || self_collision)
return { status, q_new };
Expand Down
15 changes: 12 additions & 3 deletions src/robots/xArm6.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,15 @@ void robots::xArm6::setState(const std::shared_ptr<base::State> q)
}
}

void robots::xArm6::setCapsulesRadius(const std::vector<float> &capsules_radius_)
{
capsules_radius = capsules_radius_;
capsules_radius_new = std::vector<float>(num_DOFs);
for (size_t i = 0; i < num_DOFs-1; i++)
capsules_radius_new[i] = std::max(capsules_radius[i], capsules_radius[i+1]);
capsules_radius_new.back() = capsules_radius.back();
}

std::shared_ptr<std::vector<KDL::Frame>> robots::xArm6::computeForwardKinematics(const std::shared_ptr<base::State> q)
{
setConfiguration(q);
Expand Down Expand Up @@ -241,7 +250,7 @@ std::shared_ptr<Eigen::MatrixXf> robots::xArm6::computeEnclosingRadii(const std:
{
case 0: // Special case when all frame origins are projected on {x,y} plane
if (j >= 2)
R(i, j) = std::max(R(i, j-1), skeleton->col(j).head(2).norm() + capsules_radius[j-1]);
R(i, j) = std::max(R(i, j-1), skeleton->col(j).head(2).norm() + capsules_radius_new[j-1]);
break;

case 3: // Projection of 5. and 6. skeleton point to the link [3-4] is needed
Expand All @@ -250,12 +259,12 @@ std::shared_ptr<Eigen::MatrixXf> robots::xArm6::computeEnclosingRadii(const std:
Eigen::Vector3f AB { skeleton->col(4) - skeleton->col(3) };
float t { (skeleton->col(j) - skeleton->col(3)).dot(AB) / AB.squaredNorm() };
Eigen::Vector3f proj { skeleton->col(3) + t * AB };
R(i, j) = std::max(R(i, j-1), (skeleton->col(j) - proj).norm() + capsules_radius[j-1]);
R(i, j) = std::max(R(i, j-1), (skeleton->col(j) - proj).norm() + capsules_radius_new[j-1]);
}
break;

default: // No projection is needed
R(i, j) = std::max(R(i, j-1), (skeleton->col(j) - skeleton->col(i)).norm() + capsules_radius[j-1]);
R(i, j) = std::max(R(i, j-1), (skeleton->col(j) - skeleton->col(i)).norm() + capsules_radius_new[j-1]);
break;
}
}
Expand Down
13 changes: 5 additions & 8 deletions src/state_spaces/real_vector_space/RealVectorSpace.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -433,7 +433,7 @@ float base::RealVectorSpace::computeDistanceUnderestimation(const std::shared_pt
(skeleton->col(i+1) - O).dot((R - O).normalized()))
- robot->getCapsuleRadius(i);
if (d_c_temp < 0)
return d_c_temp;
return 0;

d_c_profile[i] = std::min(d_c_profile[i], d_c_temp);

Expand All @@ -445,12 +445,9 @@ float base::RealVectorSpace::computeDistanceUnderestimation(const std::shared_pt
d_c = std::min(d_c, d_c_profile[i]);
}

if (d_c > q->getDistance()) // Also, if it was previously computed (q->getDistance() > 0), take "better" (greater) one
{
q->setDistance(d_c);
q->setDistanceProfile(d_c_profile);
q->setIsRealDistance(false);
}
q->setDistance(d_c);
q->setDistanceProfile(d_c_profile);
q->setIsRealDistance(false);

return q->getDistance();
return d_c;
}
Loading