Skip to content

Ruckig-smoothing : reduce number of duration extensions #1990

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.

Sign up for GitHub

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

Merged
AndyZe merged 7 commits into from
Mar 16, 2023
Merged

Ruckig-smoothing : reduce number of duration extensions #1990

Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
896e69b
extend duration only for failed segment
Mar 6, 2023
3814d2b
update comment
Mar 6, 2023
97da2b6
Remove trajectory reset before extension
Mar 7, 2023
dcacef1
readability improvement
Mar 7, 2023
c006722
Merge branch 'main' into ibrahim/trajectory_processing_enhancement
AndyZe Mar 13, 2023
35da797
Remove call to RobotState update
Mar 16, 2023
8ce3740
Merge branch 'main' into ibrahim/trajectory_processing_enhancement
AndyZe Mar 16, 2023
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
49 changes: 24 additions & 25 deletions moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -318,13 +318,15 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
ruckig::Result ruckig_result;
double duration_extension_factor = 1;
bool smoothing_complete = false;
size_t waypoint_idx = 0;
while ((duration_extension_factor < MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete)
{
for (size_t waypoint_idx = 0; waypoint_idx < num_waypoints - 1; ++waypoint_idx)
while (waypoint_idx < num_waypoints - 1)
{
moveit::core::RobotStatePtr curr_waypoint = trajectory.getWayPointPtr(waypoint_idx);
moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1);

getNextRuckigInput(trajectory.getWayPointPtr(waypoint_idx), next_waypoint, group, ruckig_input);
getNextRuckigInput(curr_waypoint, next_waypoint, group, ruckig_input);

// Run Ruckig
ruckig_result = ruckig.update(ruckig_input, ruckig_output);
Expand Down Expand Up @@ -354,17 +356,16 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
if (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished)
{
duration_extension_factor *= DURATION_EXTENSION_FRACTION;
// Reset the trajectory
trajectory = robot_trajectory::RobotTrajectory(original_trajectory, true /* deep copy */);

const std::vector<int>& move_group_idx = group->getVariableIndexList();
extendTrajectoryDuration(duration_extension_factor, num_waypoints, num_dof, move_group_idx, trajectory,
extendTrajectoryDuration(duration_extension_factor, waypoint_idx, num_dof, move_group_idx, trajectory,
original_trajectory);

initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input, ruckig_output);
// Begin the for() loop again
// Continue the loop from failed segment, but with increased duration extension factor
break;
}
++waypoint_idx;
}
}

Expand All @@ -377,31 +378,29 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
return true;
}

void RuckigSmoothing::extendTrajectoryDuration(const double duration_extension_factor, size_t num_waypoints,
void RuckigSmoothing::extendTrajectoryDuration(const double duration_extension_factor, size_t waypoint_idx,
const size_t num_dof, const std::vector<int>& move_group_idx,
const robot_trajectory::RobotTrajectory& original_trajectory,
robot_trajectory::RobotTrajectory& trajectory)
{
for (size_t time_stretch_idx = 1; time_stretch_idx < num_waypoints; ++time_stretch_idx)
trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1,
duration_extension_factor *
original_trajectory.getWayPointDurationFromPrevious(waypoint_idx + 1));
// re-calculate waypoint velocity and acceleration
auto target_state = trajectory.getWayPointPtr(waypoint_idx + 1);
const auto prev_state = trajectory.getWayPointPtr(waypoint_idx);

double timestep = trajectory.getWayPointDurationFromPrevious(waypoint_idx + 1);

for (size_t joint = 0; joint < num_dof; ++joint)
{
trajectory.setWayPointDurationFromPrevious(
time_stretch_idx,
duration_extension_factor * original_trajectory.getWayPointDurationFromPrevious(time_stretch_idx));
// re-calculate waypoint velocity and acceleration
auto target_state = trajectory.getWayPointPtr(time_stretch_idx);
const auto prev_state = trajectory.getWayPointPtr(time_stretch_idx - 1);
double timestep = trajectory.getAverageSegmentDuration();
for (size_t joint = 0; joint < num_dof; ++joint)
{
target_state->setVariableVelocity(move_group_idx.at(joint),
(1 / duration_extension_factor) *
target_state->getVariableVelocity(move_group_idx.at(joint)));
target_state->setVariableVelocity(move_group_idx.at(joint),
(1 / duration_extension_factor) *
target_state->getVariableVelocity(move_group_idx.at(joint)));

double prev_velocity = prev_state->getVariableVelocity(move_group_idx.at(joint));
double curr_velocity = target_state->getVariableVelocity(move_group_idx.at(joint));
target_state->setVariableAcceleration(move_group_idx.at(joint), (curr_velocity - prev_velocity) / timestep);
}
target_state->update();
double prev_velocity = prev_state->getVariableVelocity(move_group_idx.at(joint));
double curr_velocity = target_state->getVariableVelocity(move_group_idx.at(joint));
target_state->setVariableAcceleration(move_group_idx.at(joint), (curr_velocity - prev_velocity) / timestep);
}
}

Expand Down