Skip to content

Commit

Permalink
Simplification. Simply use Ruckig as a check if target state can be r…
Browse files Browse the repository at this point in the history 
…eached

Use the Ruckig constant

Simplification. Use Ruckig just as a check if the waypoint could be reached.
  • Loading branch information
AndyZe committed Mar 15, 2022
1 parent 44ffc09 commit cdc589f
Show file tree
Hide file tree
Showing 2 changed files with 29 additions and 33 deletions.
18 changes: 7 additions & 11 deletions ...t_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,11 +42,6 @@

namespace trajectory_processing
{
namespace
{
constexpr size_t RUCKIG_DYNAMIC_DOF = 0; // Put Ruckig in "dynamic DOF" mode
}

class RuckigSmoothing
{
public:
Expand All @@ -63,9 +58,10 @@ class RuckigSmoothing
* \param idx MoveIt list of joint group indices
* \param ruckig_input Output. The Rucking parameters for the next iteration
*/
static void getNextRuckigInput(const ruckig::OutputParameter<RUCKIG_DYNAMIC_DOF>& ruckig_output,
static void getNextRuckigInput(const ruckig::OutputParameter<ruckig::DynamicDOFs>& ruckig_output,
const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof,
const std::vector<int>& idx, ruckig::InputParameter<RUCKIG_DYNAMIC_DOF>& ruckig_input);
const std::vector<int>& idx,
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input);

/**
* \brief Check for leading or lagging motion of any joint at a waypoint.
Expand All @@ -75,8 +71,8 @@ class RuckigSmoothing
* \return true if leading or lagging motion is detected on any joint
*/
static bool detectLeadingOrLaggingMotion(const size_t num_dof,
const ruckig::InputParameter<RUCKIG_DYNAMIC_DOF>& ruckig_input,
const ruckig::OutputParameter<RUCKIG_DYNAMIC_DOF>& ruckig_output);
const ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
const ruckig::OutputParameter<ruckig::DynamicDOFs>& ruckig_output);

/**
* \brief Initialize Ruckig position/vel/accel. This initializes ruckig_input and ruckig_output to the same values
Expand All @@ -86,8 +82,8 @@ class RuckigSmoothing
* \param num_dof Number of actuated joints
* \param joint_idx MoveIt list of joint group indices
*/
static void initializeRuckigState(ruckig::InputParameter<RUCKIG_DYNAMIC_DOF>& ruckig_input,
ruckig::OutputParameter<RUCKIG_DYNAMIC_DOF>& ruckig_output,
static void initializeRuckigState(ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
ruckig::OutputParameter<ruckig::DynamicDOFs>& ruckig_output,
const moveit::core::RobotState& first_waypoint, size_t num_dof,
const std::vector<int>& joint_idx);
};
Expand Down
44 changes: 22 additions & 22 deletions moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,10 +82,10 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto

// Instantiate the smoother
double timestep = trajectory.getWayPointDurationFromStart(num_waypoints - 1) / (trajectory.getWayPointCount() - 1);
std::unique_ptr<ruckig::Ruckig<RUCKIG_DYNAMIC_DOF>> ruckig_ptr;
ruckig_ptr = std::make_unique<ruckig::Ruckig<RUCKIG_DYNAMIC_DOF>>(num_dof, timestep);
ruckig::InputParameter<RUCKIG_DYNAMIC_DOF> ruckig_input{ num_dof };
ruckig::OutputParameter<RUCKIG_DYNAMIC_DOF> ruckig_output{ num_dof };
std::unique_ptr<ruckig::Ruckig<ruckig::DynamicDOFs>> ruckig_ptr;
ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
ruckig::OutputParameter<ruckig::DynamicDOFs> ruckig_output{ num_dof };

// Initialize the smoother
const std::vector<int>& move_group_idx = group->getVariableIndexList();
Expand Down Expand Up @@ -121,8 +121,8 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
}

ruckig::Result ruckig_result;
bool smoothing_complete = false;
double duration_extension_factor = 1;
bool smoothing_complete = false;
while ((duration_extension_factor < MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete)
{
for (size_t waypoint_idx = 0; waypoint_idx < num_waypoints - 1; ++waypoint_idx)
Expand All @@ -134,17 +134,14 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
// Run Ruckig
ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);

smoothing_complete = (ruckig_result == ruckig::Result::Finished);
if (smoothing_complete)
if ((waypoint_idx = num_waypoints - 2) && ruckig_result == ruckig::Result::Finished)
{
smoothing_complete = true;
break;
}

// If the requested velocity is too great, a joint can actually "move backward" to give itself more time to
// accelerate to the target velocity (or way-overshoot to do the same).
// Iterate and decrease velocities until that behavior is gone.
bool lead_or_lag_detected = detectLeadingOrLaggingMotion(num_dof, ruckig_input, ruckig_output);
if (lead_or_lag_detected)
// Extend the trajectory duration if Ruckig could not reach the waypoint successfully
if (ruckig_result != ruckig::Result::Finished)
{
duration_extension_factor *= DURATION_EXTENSION_FRACTION;
// Reset the trajectory
Expand All @@ -156,18 +153,21 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
duration_extension_factor * original_trajectory.getWayPointDurationFromPrevious(time_stretch_idx));
// re-calculate waypoint velocity and acceleration
auto target_state = trajectory.getWayPointPtr(time_stretch_idx);
auto const prev_state = trajectory.getWayPointPtr(time_stretch_idx - 1);
timestep = trajectory.getWayPointDurationFromStart(num_waypoints - 1) / (trajectory.getWayPointCount() - 1);
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->setVariableAcceleration(move_group_idx.at(joint), 0);

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();
}

timestep = trajectory.getWayPointDurationFromStart(num_waypoints - 1) / (trajectory.getWayPointCount() - 1);
ruckig_ptr = std::make_unique<ruckig::Ruckig<RUCKIG_DYNAMIC_DOF>>(num_dof, timestep);
ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
waypoint_idx = 0;
initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), num_dof, move_group_idx);
break;
Expand All @@ -186,8 +186,8 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
return true;
}

void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<RUCKIG_DYNAMIC_DOF>& ruckig_input,
ruckig::OutputParameter<RUCKIG_DYNAMIC_DOF>& ruckig_output,
void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
ruckig::OutputParameter<ruckig::DynamicDOFs>& ruckig_output,
const moveit::core::RobotState& first_waypoint, size_t num_dof,
const std::vector<int>& idx)
{
Expand All @@ -211,8 +211,8 @@ void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<RUCKIG_DYNAMI
}

bool RuckigSmoothing::detectLeadingOrLaggingMotion(const size_t num_dof,
const ruckig::InputParameter<RUCKIG_DYNAMIC_DOF>& ruckig_input,
const ruckig::OutputParameter<RUCKIG_DYNAMIC_DOF>& ruckig_output)
const ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
const ruckig::OutputParameter<ruckig::DynamicDOFs>& ruckig_output)
{
for (size_t joint = 0; joint < num_dof; ++joint)
{
Expand All @@ -228,10 +228,10 @@ bool RuckigSmoothing::detectLeadingOrLaggingMotion(const size_t num_dof,
return false;
}

void RuckigSmoothing::getNextRuckigInput(const ruckig::OutputParameter<RUCKIG_DYNAMIC_DOF>& ruckig_output,
void RuckigSmoothing::getNextRuckigInput(const ruckig::OutputParameter<ruckig::DynamicDOFs>& ruckig_output,
const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof,
const std::vector<int>& idx,
ruckig::InputParameter<RUCKIG_DYNAMIC_DOF>& ruckig_input)
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
{
// TODO(andyz): https://github.com/ros-planning/moveit2/issues/766
// ruckig_output.pass_to_input(ruckig_input);
Expand Down

0 comments on commit cdc589f

Please sign in to comment.