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.

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

Jump to bottom

Replace num_dof and idx variables with JointGroup API #1152

Merged
merged 9 commits into from
Apr 12, 2022
Merged

Replace num_dof and idx variables with JointGroup API #1152

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
21535a6
Replace variables obtainable with the JointGroup API
burkap Mar 30, 2022
c93987f
Get rid of signedness mismatch warning
burkap Mar 30, 2022
ac5c960
Merge branch 'main' into replace-variables
Mar 31, 2022
a47496d
Merge branch 'main' into replace-variables
Mar 31, 2022
9b4d76d
Doxygen alignment
AndyZe Apr 5, 2022
5c1957b
Merge branch 'main' into replace-variables
AndyZe Apr 5, 2022
5817298
Merge branch 'main' into replace-variables
AndyZe Apr 5, 2022
1570f93
Merge branch 'main' into replace-variables
Apr 6, 2022
476ee2b
Merge branch 'main' into replace-variables
AndyZe Apr 8, 2022
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
25 changes: 13 additions & 12 deletions ...t_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
View file
Open in desktop
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,
AndyZe marked this conversation as resolved.
Show resolved Hide resolved
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
Open in desktop
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
Open in desktop
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