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

Fixup TOTG #3427

Merged
merged 10 commits into from May 12, 2023
Merged

Fixup TOTG #3427

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
cffb33f
Minor cleanup and debugging in TOTG
AndyZe May 10, 2023
f77e1c9
fixup
rhaschke May 11, 2023
17cbe9b
Add TOTG test for a robot with a mimic joint
rhaschke May 11, 2023
5c2bfb4
Reduce code redundancy + fix indexing issues
rhaschke May 11, 2023
4abdb38
Simplify code: drop std::optional
rhaschke May 11, 2023
7419742
Simplify robot description in test
rhaschke May 11, 2023
c06b1c2
shorten test: reduce amount of joint motion
rhaschke May 12, 2023
e3ff458
Addressed review comments
rhaschke May 12, 2023
4b7a29a
Further reduce code redundancy
rhaschke May 12, 2023
801ea9c
Throttle warnings
rhaschke May 12, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
4 changes: 0 additions & 4 deletions moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h
View file
Open in desktop
Expand Up @@ -581,10 +581,6 @@ class JointModelGroup
bool isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose, std::size_t array_size,
double dt) const;

/** \brief Computes the indices of joint variables given a vector of joint names to look up */
bool computeJointVariableIndices(const std::vector<std::string>& joint_names,
std::vector<size_t>& joint_bijection) const;

protected:
bool computeIKIndexBijection(const std::vector<std::string>& ik_jnames,
std::vector<unsigned int>& joint_bijection) const;
Expand Down
25 changes: 0 additions & 25 deletions moveit_core/robot_model/src/joint_model_group.cpp
View file
Open in desktop
Expand Up @@ -558,31 +558,6 @@ void JointModelGroup::setDefaultIKTimeout(double ik_timeout)
it.second.default_ik_timeout_ = ik_timeout;
}

bool JointModelGroup::computeJointVariableIndices(const std::vector<std::string>& joint_names,
std::vector<size_t>& joint_bijection) const
{
joint_bijection.clear();
for (const std::string& joint_name : joint_names)
{
VariableIndexMap::const_iterator it = joint_variables_index_map_.find(joint_name);
if (it == joint_variables_index_map_.end())
{
// skip reported fixed joints
if (hasJointModel(joint_name) && getJointModel(joint_name)->getType() == JointModel::FIXED)
continue;
ROS_ERROR_NAMED(LOGNAME,
"Looking for variables for joint '%s', "
"but group '%s' does not contain such a joint.",
joint_name.c_str(), getName().c_str());
return false;
}
const JointModel* jm = getJointModel(joint_name);
for (size_t k = 0; k < jm->getVariableCount(); ++k)
joint_bijection.push_back(it->second + k);
}
return true;
}

bool JointModelGroup::computeIKIndexBijection(const std::vector<std::string>& ik_jnames,
std::vector<unsigned int>& joint_bijection) const
{
Expand Down
2 changes: 1 addition & 1 deletion ...processing/include/moveit/trajectory_processing/iterative_torque_limit_parameterization.h
View file
Open in desktop
Expand Up @@ -73,6 +73,6 @@ class IterativeTorqueLimitParameterization
const double max_acceleration_scaling_factor) const;

private:
std::optional<TimeOptimalTrajectoryGeneration> totg_;
TimeOptimalTrajectoryGeneration totg_;
};
} // namespace trajectory_processing
10 changes: 5 additions & 5 deletions ...tory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h
View file
Open in desktop
Expand Up @@ -188,7 +188,11 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
* \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
*/
bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0) const override;
const double max_acceleration_scaling_factor = 1.0) const override
{
std::unordered_map<std::string, double> empty;
return computeTimeStamps(trajectory, empty, empty, max_velocity_scaling_factor, max_acceleration_scaling_factor);
}

/**
* \brief Compute a trajectory with waypoints spaced equally in time (according to resample_dt_).
Expand All @@ -211,10 +215,6 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
const double max_acceleration_scaling_factor = 1.0) const;

private:
bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory,
const Eigen::VectorXd& max_velocity,
const Eigen::VectorXd& max_acceleration) const;

/**
* @brief Check if a combination of revolute and prismatic joints is used. path_tolerance_ is not valid, if so.
* \param group The JointModelGroup to check.
Expand Down
12 changes: 3 additions & 9 deletions moveit_core/trajectory_processing/src/iterative_torque_limit_parameterization.cpp
View file
Open in desktop
Expand Up @@ -46,8 +46,8 @@ const std::string LOGNAME = "trajectory_processing.iterative_torque_limit_parame
IterativeTorqueLimitParameterization::IterativeTorqueLimitParameterization(const double path_tolerance,
const double resample_dt,
const double min_angle_change)
: totg_(path_tolerance, resample_dt, min_angle_change)
{
totg_.emplace(TimeOptimalTrajectoryGeneration(path_tolerance, resample_dt, min_angle_change));
}

bool IterativeTorqueLimitParameterization::computeTimeStampsWithTorqueLimits(
Expand Down Expand Up @@ -101,19 +101,13 @@ bool IterativeTorqueLimitParameterization::computeTimeStampsWithTorqueLimits(
size_t num_iterations = 0;
const size_t max_iterations = 10;

if (!totg_)
{
ROS_ERROR_NAMED(LOGNAME, "The totg_ member was not initialized.");
return false;
}

while (iteration_needed && num_iterations < max_iterations)
{
++num_iterations;
iteration_needed = false;

totg_->computeTimeStamps(trajectory, velocity_limits, mutable_accel_limits, max_velocity_scaling_factor,
max_acceleration_scaling_factor);
totg_.computeTimeStamps(trajectory, velocity_limits, mutable_accel_limits, max_velocity_scaling_factor,
max_acceleration_scaling_factor);

std::vector<double> joint_positions(dof);
std::vector<double> joint_velocities(dof);
Expand Down
196 changes: 47 additions & 149 deletions moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
View file
Open in desktop
Expand Up @@ -862,78 +862,6 @@ TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(const double pa
{
}

bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
const double max_velocity_scaling_factor,
const double max_acceleration_scaling_factor) const
{
if (trajectory.empty())
return true;

const moveit::core::JointModelGroup* group = trajectory.getGroup();
if (!group)
{
ROS_ERROR_NAMED(LOGNAME, "It looks like the planner did not set the group the plan was computed for");
return false;
}

// Validate scaling
double velocity_scaling_factor = verifyScalingFactor(max_velocity_scaling_factor);
double acceleration_scaling_factor = verifyScalingFactor(max_acceleration_scaling_factor);

// Get the velocity and acceleration limits for all active joints
const moveit::core::RobotModel& rmodel = group->getParentModel();
const std::vector<std::string>& vars = group->getVariableNames();
std::vector<size_t> active_joint_indices;
if (!group->computeJointVariableIndices(group->getActiveJointModelNames(), active_joint_indices))
{
ROS_ERROR_NAMED(LOGNAME, "Failed to get active variable indices.");
}

const size_t num_active_joints = active_joint_indices.size();
Eigen::VectorXd max_velocity(num_active_joints);
Eigen::VectorXd max_acceleration(num_active_joints);
for (size_t idx = 0; idx < num_active_joints; ++idx)
{
// For active joints only (skip mimic joints and other types)
const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[active_joint_indices[idx]]);

// Limits need to be non-zero, otherwise we never exit
if (bounds.velocity_bounded_)
{
if (bounds.max_velocity_ <= 0.0)
{
ROS_ERROR_NAMED(LOGNAME, "Invalid max_velocity %f specified for '%s', must be greater than 0.0",
bounds.max_velocity_, vars[idx].c_str());
return false;
}
max_velocity[idx] =
std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor;
}
else
{
max_velocity[idx] = DEFAULT_VELOCITY_LIMIT;
}

if (bounds.acceleration_bounded_)
{
if (bounds.max_acceleration_ < 0.0)
{
ROS_ERROR_NAMED(LOGNAME, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0",
bounds.max_acceleration_, vars[idx].c_str());
return false;
}
max_acceleration[idx] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) *
acceleration_scaling_factor;
}
else
{
max_acceleration[idx] = DEFAULT_ACCELERATION_LIMIT;
}
}

return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration);
}

bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map<std::string, double>& velocity_limits,
const std::unordered_map<std::string, double>& acceleration_limits, const double max_velocity_scaling_factor,
Expand All @@ -942,7 +870,6 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
if (trajectory.empty())
return true;

// Get the default joint limits from the robot model, then overwrite any that are provided as arguments
const moveit::core::JointModelGroup* group = trajectory.getGroup();
if (!group)
{
Expand All @@ -954,118 +881,89 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
double velocity_scaling_factor = verifyScalingFactor(max_velocity_scaling_factor);
double acceleration_scaling_factor = verifyScalingFactor(max_acceleration_scaling_factor);

// Get the velocity and acceleration limits for specified joints
// limits need to be positive, otherwise we never exit
auto validate_limit = [](const char* type, double value, const std::string& name) {
if (value <= std::numeric_limits<double>::epsilon())
{
ROS_ERROR_NAMED(LOGNAME, "Invalid %s limit %f for joint '%s'. Must be greater than zero!", type, value,
name.c_str());
return false;
}
return true;
};

// Get the velocity and acceleration limits for all joint variables
const moveit::core::RobotModel& rmodel = group->getParentModel();
const std::vector<std::string>& vars = group->getVariableNames();
std::vector<size_t> indices;
if (!group->computeJointVariableIndices(group->getActiveJointModelNames(), indices))
{
ROS_ERROR_NAMED(LOGNAME, "Failed to get active variable indices.");
}

const size_t num_joints = indices.size();

const auto num_joints = group->getVariableCount();
Eigen::VectorXd max_velocity(num_joints);
Eigen::VectorXd max_acceleration(num_joints);
for (const auto idx : indices)
for (size_t j = 0; j < num_joints; ++j)
{
const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[idx]);
const auto& name = vars[j];
const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(name);

// VELOCITY LIMIT
// Check if a custom limit was specified as an argument
bool set_velocity_limit = false;
auto it = velocity_limits.find(vars[idx]);
auto it = velocity_limits.find(name); // check for a custom limit
if (it != velocity_limits.end())
{
max_velocity[idx] = it->second * velocity_scaling_factor;
set_velocity_limit = true;
if (!validate_limit("velocity", it->second, name))
return false;
max_velocity[j] = it->second * velocity_scaling_factor;
}

if (bounds.velocity_bounded_ && !set_velocity_limit)
else if (bounds.velocity_bounded_) // resort to the limit from the robot model
{
// Set the default velocity limit, from robot model
if (bounds.max_velocity_ < 0.0)
{
ROS_ERROR_NAMED(LOGNAME, "Invalid max_velocity %f specified for '%s', must be greater than 0.0",
bounds.max_velocity_, vars[idx].c_str());
if (!validate_limit("velocity", bounds.max_velocity_, name))
return false;
}
max_velocity[idx] =
max_velocity[j] =
std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor;
set_velocity_limit = true;
}

if (!set_velocity_limit)
else // default limit
{
ROS_ERROR_STREAM_NAMED(LOGNAME, "No velocity limit was defined for joint "
<< vars[idx].c_str()
<< "! You have to define velocity limits in the URDF or "
"joint_limits.yaml");
return false;
max_velocity[j] = DEFAULT_VELOCITY_LIMIT;
ROS_WARN_ONCE_NAMED(LOGNAME, "No velocity limits defined for '%s'! Define them in URDF or joint_limits.yaml",
name.c_str());
}

// ACCELERATION LIMIT
// Check if a custom limit was specified as an argument
bool set_acceleration_limit = false;
it = acceleration_limits.find(vars[idx]);
it = acceleration_limits.find(name); // check for a custom limit
if (it != acceleration_limits.end())
{
max_acceleration[idx] = it->second * acceleration_scaling_factor;
set_acceleration_limit = true;
if (!validate_limit("acceleration", it->second, name))
return false;
max_acceleration[j] = it->second * acceleration_scaling_factor;
rhaschke marked this conversation as resolved.
Show resolved Hide resolved
}

if (bounds.acceleration_bounded_ && !set_acceleration_limit)
else if (bounds.acceleration_bounded_) // resort to the limit from the robot model
{
// Set the default acceleration limit, from robot model
if (bounds.max_acceleration_ < 0.0)
{
ROS_ERROR_NAMED(LOGNAME, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0",
bounds.max_acceleration_, vars[idx].c_str());
if (!validate_limit("acceleration", bounds.max_acceleration_, name))
return false;
}
max_acceleration[idx] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) *
acceleration_scaling_factor;
set_acceleration_limit = true;
max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) *
acceleration_scaling_factor;
}
if (!set_acceleration_limit)
else // default limit
{
ROS_ERROR_STREAM_NAMED(LOGNAME, "No acceleration limit was defined for joint "
<< vars[idx].c_str()
<< "! You have to define acceleration limits in the URDF or "
"joint_limits.yaml");
return false;
max_acceleration[j] = DEFAULT_ACCELERATION_LIMIT;
ROS_WARN_ONCE_NAMED(LOGNAME, "No acceleration limits defined for '%s'! Define them in joint_limits.yaml",
name.c_str());
}
}

return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration);
}

bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory,
const Eigen::VectorXd& max_velocity,
const Eigen::VectorXd& max_acceleration) const
{
// This lib does not actually work properly when angles wrap around, so we need to unwind the path first
trajectory.unwind();

const moveit::core::JointModelGroup* group = trajectory.getGroup();
if (!group)
{
ROS_ERROR_NAMED(LOGNAME, "It looks like the planner did not set the group the plan was computed for");
return false;
}

if (hasMixedJointTypes(group))
{
ROS_WARN_NAMED(LOGNAME, "There is a combination of revolute and prismatic joints in the robot model. TOTG's "
"`path_tolerance` will not function correctly.");
ROS_WARN_ONCE_NAMED(LOGNAME,
"There is a combination of revolute and prismatic joints in the robot model. "
"TOTG's `path_tolerance` parameter is applied to both types ignoring their different units.");
}

const unsigned num_points = trajectory.getWayPointCount();
const std::vector<int>& idx = group->getVariableIndexList();
const unsigned num_joints = group->getVariableCount();

// Have to convert into Eigen data structs and remove repeated points
// (https://github.com/tobiaskunz/trajectories/issues/3)
// (https://github.com/tobiaskunz/trajectories/issues/3)
std::list<Eigen::VectorXd> points;
for (size_t p = 0; p < num_points; ++p)
{
Expand Down Expand Up @@ -1164,11 +1062,11 @@ bool TimeOptimalTrajectoryGeneration::hasMixedJointTypes(const moveit::core::Joi

double TimeOptimalTrajectoryGeneration::verifyScalingFactor(const double requested_scaling_factor) const
{
double scaling_factor = std::clamp(requested_scaling_factor, -1.0, 1.0);

if (requested_scaling_factor > 1.0 || requested_scaling_factor < -1.0)
double scaling_factor = std::clamp(requested_scaling_factor, 1e-7, 1.0);
if (requested_scaling_factor != scaling_factor)
{
ROS_WARN_NAMED(LOGNAME, "Invalid max_scaling_factor specified, defaulting to %f instead.", scaling_factor);
ROS_WARN_NAMED(LOGNAME, "Invalid max_scaling_factor specified: %f, reverting to %f instead.",
requested_scaling_factor, scaling_factor);
}
return scaling_factor;
}
Expand Down