Skip to content

Commit

Permalink
Replace num_dof and idx variables with JointGroup API (#1152)
Browse files Browse the repository at this point in the history
  • Loading branch information
@burkap
burkap committed Apr 12, 2022
1 parent a0c4d34 commit 3fe5b82
Show file tree
Hide file tree
Showing 3 changed files with 48 additions and 31 deletions.
25 changes: 13 additions & 12 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 @@ -54,30 +54,32 @@ class RuckigSmoothing
* \brief Feed previous output back as input for next iteration. Get next target state from the next waypoint.
* \param ruckig_output The previous output from Ruckig
* \param next_waypoint The nominal, desired state at the next waypoint
* \param num_dof Number of actuated joints
* \param idx MoveIt list of joint group indices
* \param joint_group The MoveIt JointModelGroup of interest
* \param ruckig_input Output. The Rucking parameters for the next iteration
*/
static void getNextRuckigInput(const ruckig::OutputParameter<0>& ruckig_output,
const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof,
const std::vector<int>& idx, ruckig::InputParameter<0>& ruckig_input);
const moveit::core::RobotStatePtr& next_waypoint,
const moveit::core::JointModelGroup* joint_group,
ruckig::InputParameter<0>& ruckig_input);

/**
* \brief Check for lagging motion of any joint at a waypoint.
* \param num_dof Number of actuated joints
* \param joint_group The MoveIt JointModelGroup of interest
* \param ruckig_input Input parameters to Ruckig
* \param ruckig_output Output parameters from Ruckig
* \return true if lagging motion is detected on any joint
*/
static bool checkForLaggingMotion(const size_t num_dof, const ruckig::InputParameter<0>& ruckig_input,
static bool checkForLaggingMotion(const moveit::core::JointModelGroup* joint_group,
const ruckig::InputParameter<0>& ruckig_input,
const ruckig::OutputParameter<0>& ruckig_output);

/**
* \brief Return L2-norm of velocity, taking all joints into account.
* \param ruckig_input Input parameters to Ruckig
* \param num_dof Number of actuated joints
* \param joint_group The MoveIt JointModelGroup of interest
*/
static double getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input, size_t num_dof);
static double getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input,
const moveit::core::JointModelGroup* joint_group);

/**
* \brief Check if the joint positions of two waypoints are very similar.
Expand All @@ -94,11 +96,10 @@ class RuckigSmoothing
* \param rucking_input Input parameters to Ruckig. Initialized here.
* \param ruckig_output Output from the Ruckig algorithm. Initialized here.
* \param first_waypoint The Ruckig input/output parameters are initialized to the values at this waypoint
* \param num_dof Number of actuated joints
* \param joint_idx MoveIt list of joint group indices
* \param joint_group The MoveIt JointModelGroup of interest
*/
static void initializeRuckigState(ruckig::InputParameter<0>& ruckig_input, ruckig::OutputParameter<0>& ruckig_output,
const moveit::core::RobotState& first_waypoint, size_t num_dof,
const std::vector<int>& joint_idx);
const moveit::core::RobotState& first_waypoint,
const moveit::core::JointModelGroup* joint_group);
};
} // namespace trajectory_processing
17 changes: 11 additions & 6 deletions moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ struct SingleJointTrajectory
double max_acceleration_;
};

void globalAdjustment(std::vector<SingleJointTrajectory>& t2, int num_joints, const int num_points,
void globalAdjustment(std::vector<SingleJointTrajectory>& t2, robot_trajectory::RobotTrajectory& trajectory,
std::vector<double>& time_diff);

IterativeSplineParameterization::IterativeSplineParameterization(bool add_points) : add_points_(add_points)
Expand Down Expand Up @@ -321,7 +321,7 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT
}

// Final adjustment forces the trajectory within bounds
globalAdjustment(t2, num_joints, num_points, time_diff);
globalAdjustment(t2, trajectory, time_diff);

// Convert back to JointTrajectory form
for (unsigned int i = 1; i < num_points; ++i)
Expand Down Expand Up @@ -570,11 +570,16 @@ static double global_adjustment_factor(const int n, double x1[], double x2[], co
}

// Expands the entire trajectory to fit exactly within bounds
void globalAdjustment(std::vector<SingleJointTrajectory>& t2, int num_joints, const int num_points,
void globalAdjustment(std::vector<SingleJointTrajectory>& t2, robot_trajectory::RobotTrajectory& trajectory,
std::vector<double>& time_diff)
{
const moveit::core::JointModelGroup* group = trajectory.getGroup();

const unsigned int num_points = trajectory.getWayPointCount();
const unsigned int num_joints = group->getVariableCount();

double gtfactor = 1.0;
for (int j = 0; j < num_joints; ++j)
for (unsigned int j = 0; j < num_joints; ++j)
{
double tfactor;
tfactor = global_adjustment_factor(num_points, &t2[j].velocities_[0], &t2[j].accelerations_[0], t2[j].max_velocity_,
Expand All @@ -584,10 +589,10 @@ void globalAdjustment(std::vector<SingleJointTrajectory>& t2, int num_joints, co
}

// printf("# Global adjustment: %0.4f%%\n", 100.0 * (gtfactor - 1.0));
for (int i = 0; i < num_points - 1; ++i)
for (unsigned int i = 0; i < num_points - 1; ++i)
time_diff[i] *= gtfactor;

for (int j = 0; j < num_joints; ++j)
for (unsigned int j = 0; j < num_joints; ++j)
{
fit_cubic_spline(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], &t2[j].accelerations_[0]);
}
Expand Down
37 changes: 24 additions & 13 deletions moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto

// Initialize the smoother
const std::vector<int>& idx = group->getVariableIndexList();
initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), num_dof, idx);
initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);

// Kinematic limits (vel/accel/jerk)
const std::vector<std::string>& vars = group->getVariableNames();
Expand Down Expand Up @@ -127,16 +127,16 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
{
moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1);

getNextRuckigInput(ruckig_output, next_waypoint, num_dof, idx, ruckig_input);
getNextRuckigInput(ruckig_output, next_waypoint, group, ruckig_input);

// Run Ruckig
ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);

// If the requested velocity is too great, a joint can actually "move backward" to give itself more time to
// accelerate to the target velocity. Iterate and decrease velocities until that behavior is gone.
bool backward_motion_detected = checkForLaggingMotion(num_dof, ruckig_input, ruckig_output);
bool backward_motion_detected = checkForLaggingMotion(group, ruckig_input, ruckig_output);

double velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, num_dof);
double velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, group);
while (backward_motion_detected && (velocity_magnitude > MINIMUM_VELOCITY_SEARCH_MAGNITUDE))
{
// Skip repeated waypoints with no change in position. Ruckig does not handle this well and there's really no
Expand All @@ -156,12 +156,12 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
ruckig_input.target_acceleration.at(joint) =
(ruckig_input.target_velocity.at(joint) - ruckig_output.new_velocity.at(joint)) / timestep;
}
velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, num_dof);
velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, group);
// Run Ruckig
ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);

// check for backward motion
backward_motion_detected = checkForLaggingMotion(num_dof, ruckig_input, ruckig_output);
backward_motion_detected = checkForLaggingMotion(group, ruckig_input, ruckig_output);
}

// Overwrite pos/vel/accel of the target waypoint
Expand All @@ -185,7 +185,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
{
// If Ruckig failed, it's likely because the original seed trajectory did not have a long enough duration when
// jerk is taken into account. Extend the duration and try again.
initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), num_dof, idx);
initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);
duration_extension_factor *= DURATION_EXTENSION_FRACTION;
for (size_t waypoint_idx = 1; waypoint_idx < num_waypoints; ++waypoint_idx)
{
Expand All @@ -212,9 +212,12 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto

void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<0>& ruckig_input,
ruckig::OutputParameter<0>& ruckig_output,
const moveit::core::RobotState& first_waypoint, size_t num_dof,
const std::vector<int>& idx)
const moveit::core::RobotState& first_waypoint,
const moveit::core::JointModelGroup* joint_group)
{
const size_t num_dof = joint_group->getVariableCount();
const std::vector<int>& idx = joint_group->getVariableIndexList();

std::vector<double> current_positions_vector(num_dof);
std::vector<double> current_velocities_vector(num_dof);
std::vector<double> current_accelerations_vector(num_dof);
Expand Down Expand Up @@ -243,8 +246,10 @@ bool RuckigSmoothing::checkForIdenticalWaypoints(const moveit::core::RobotState&
return (magnitude_position_difference <= IDENTICAL_POSITION_EPSILON);
}

double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input, size_t num_dof)
double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input,
const moveit::core::JointModelGroup* joint_group)
{
const size_t num_dof = joint_group->getVariableCount();
double vel_magnitude = 0;
for (size_t joint = 0; joint < num_dof; ++joint)
{
Expand All @@ -253,9 +258,11 @@ double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter<
return sqrt(vel_magnitude);
}

bool RuckigSmoothing::checkForLaggingMotion(const size_t num_dof, const ruckig::InputParameter<0>& ruckig_input,
bool RuckigSmoothing::checkForLaggingMotion(const moveit::core::JointModelGroup* joint_group,
const ruckig::InputParameter<0>& ruckig_input,
const ruckig::OutputParameter<0>& ruckig_output)
{
const size_t num_dof = joint_group->getVariableCount();
// Check for backward motion of any joint
for (size_t joint = 0; joint < num_dof; ++joint)
{
Expand All @@ -269,12 +276,16 @@ bool RuckigSmoothing::checkForLaggingMotion(const size_t num_dof, const ruckig::
}

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

const size_t num_dof = joint_group->getVariableCount();
const std::vector<int>& idx = joint_group->getVariableIndexList();

for (size_t joint = 0; joint < num_dof; ++joint)
{
// Feed output from the previous timestep back as input
Expand Down

0 comments on commit 3fe5b82

Please sign in to comment.