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

Fix mimic joints with TOTG #1989

Merged
merged 3 commits into from
Mar 15, 2023
Merged

Fix mimic joints with TOTG #1989

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
df696b9
Fix mimic joints with TOTG
sea-bass Mar 4, 2023
0b98ef9
Switch to computeIKIndexBijection and revert JMG interface
sea-bass Mar 10, 2023
a9b1523
Formatting
sea-bass Mar 10, 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
6 changes: 4 additions & 2 deletions moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -582,9 +582,11 @@ class JointModelGroup
bool isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose, std::size_t array_size,
double dt) const;

protected:
bool computeIKIndexBijection(const std::vector<std::string>& ik_jnames, std::vector<size_t>& joint_bijection) 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:
/** \brief Update the variable values for the state of a group with respect to the mimic joints. This only updates
mimic joints that have the parent in this group. If there is a joint mimicking one that is outside the group,
there are no values to be read (\e values is only the group state) */
Expand Down
26 changes: 13 additions & 13 deletions moveit_core/robot_model/src/joint_model_group.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -138,6 +138,7 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode
if (vc > 1)
is_single_dof_ = false;
const std::vector<std::string>& name_order = joint_model->getVariableNames();

if (joint_model->getMimic() == nullptr)
{
active_joint_model_vector_.push_back(joint_model);
Expand Down Expand Up @@ -605,25 +606,25 @@ void JointModelGroup::setDefaultIKTimeout(double ik_timeout)
it.second.default_ik_timeout_ = ik_timeout;
}

bool JointModelGroup::computeIKIndexBijection(const std::vector<std::string>& ik_jnames,
std::vector<size_t>& joint_bijection) const
bool JointModelGroup::computeJointVariableIndices(const std::vector<std::string>& joint_names,
std::vector<size_t>& joint_bijection) const
{
joint_bijection.clear();
for (const std::string& ik_jname : ik_jnames)
for (const std::string& joint_name : joint_names)
{
VariableIndexMap::const_iterator it = joint_variables_index_map_.find(ik_jname);
VariableIndexMap::const_iterator it = joint_variables_index_map_.find(joint_name);
if (it == joint_variables_index_map_.end())
{
// skip reported fixed joints
if (hasJointModel(ik_jname) && getJointModel(ik_jname)->getType() == JointModel::FIXED)
if (hasJointModel(joint_name) && getJointModel(joint_name)->getType() == JointModel::FIXED)
continue;
RCLCPP_ERROR(LOGGER,
"IK solver computes joint values for joint '%s' "
"Looking for variables for joint '%s', "
"but group '%s' does not contain such a joint.",
ik_jname.c_str(), getName().c_str());
joint_name.c_str(), getName().c_str());
return false;
}
const JointModel* jm = getJointModel(ik_jname);
const JointModel* jm = getJointModel(joint_name);
for (size_t k = 0; k < jm->getVariableCount(); ++k)
joint_bijection.push_back(it->second + k);
}
Expand All @@ -639,8 +640,8 @@ void JointModelGroup::setSolverAllocators(const std::pair<SolverAllocatorFn, Sol
if (group_kinematics_.first.solver_instance_)
{
group_kinematics_.first.solver_instance_->setDefaultTimeout(group_kinematics_.first.default_ik_timeout_);
if (!computeIKIndexBijection(group_kinematics_.first.solver_instance_->getJointNames(),
group_kinematics_.first.bijection_))
if (!computeJointVariableIndices(group_kinematics_.first.solver_instance_->getJointNames(),
group_kinematics_.first.bijection_))
group_kinematics_.first.reset();
}
}
Expand All @@ -655,7 +656,7 @@ void JointModelGroup::setSolverAllocators(const std::pair<SolverAllocatorFn, Sol
ks.allocator_ = it.second;
ks.solver_instance_ = const_cast<JointModelGroup*>(it.first)->getSolverInstance();
ks.default_ik_timeout_ = group_kinematics_.first.default_ik_timeout_;
if (!computeIKIndexBijection(ks.solver_instance_->getJointNames(), ks.bijection_))
if (!computeJointVariableIndices(ks.solver_instance_->getJointNames(), ks.bijection_))
{
group_kinematics_.second.clear();
break;
Expand Down Expand Up @@ -730,8 +731,7 @@ void JointModelGroup::printGroupInfo(std::ostream& out) const
out << '\n';
out << " " << parent_model_->getVariableBounds(variable_name) << '\n';
}
out << " * Variables Index List:\n";
out << " ";
out << " * Variables Index List:\n ";
for (int variable_index : variable_index_list_)
out << variable_index << ' ';
if (is_contiguous_index_list_)
Expand Down
2 changes: 1 addition & 1 deletion moveit_core/robot_model/src/robot_model.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1550,7 +1550,7 @@ void RobotModel::setKinematicsAllocators(const std::map<std::string, SolverAlloc
std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(),
std::inserter(joint_model_set, joint_model_set.end()));
// TODO: instead of maintaining disjoint joint sets here,
// should we leave that work to JMG's setSolverAllocators() / computeIKIndexBijection()?
// should we leave that work to JMG's setSolverAllocators() / computeJointVariableIndices()?
// There, a disjoint bijection from joints to solvers is computed anyway.
// Underlying question: How do we resolve overlaps? Now the first considered sub group "wins"
// But, if the overlap only involves fixed joints, we could consider all sub groups
Expand Down
68 changes: 40 additions & 28 deletions moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -884,33 +884,38 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
double velocity_scaling_factor = verifyScalingFactor(max_velocity_scaling_factor, VELOCITY);
double acceleration_scaling_factor = verifyScalingFactor(max_acceleration_scaling_factor, ACCELERATION);

const std::vector<std::string>& vars = group->getVariableNames();
// Get the velocity and acceleration limits for all active joints
henningkayser marked this conversation as resolved.
Show resolved Hide resolved
const moveit::core::RobotModel& rmodel = group->getParentModel();
const unsigned num_joints = group->getVariableCount();
const std::vector<std::string>& vars = group->getVariableNames();
std::vector<size_t> indices;
if (!group->computeJointVariableIndices(group->getActiveJointModelNames(), indices))
{
RCLCPP_ERROR(LOGGER, "Failed to get active variable indices.");
}

// Get the vel/accel limits
const size_t num_joints = indices.size();
Eigen::VectorXd max_velocity(num_joints);
Eigen::VectorXd max_acceleration(num_joints);
for (size_t j = 0; j < num_joints; ++j)
for (const auto idx : indices)
{
const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]);
const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[idx]);

// Limits need to be non-zero, otherwise we never exit
if (bounds.velocity_bounded_)
{
if (bounds.max_velocity_ <= 0.0)
{
RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0",
bounds.max_velocity_, vars[j].c_str());
bounds.max_velocity_, vars[idx].c_str());
return false;
}
max_velocity[j] =
max_velocity[idx] =
std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor;
}
else
{
RCLCPP_ERROR_STREAM(LOGGER, "No velocity limit was defined for joint "
<< vars[j].c_str()
<< vars[idx].c_str()
<< "! You have to define velocity limits in the URDF or joint_limits.yaml");
return false;
}
Expand All @@ -920,16 +925,16 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
if (bounds.max_acceleration_ < 0.0)
{
RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0",
bounds.max_acceleration_, vars[j].c_str());
bounds.max_acceleration_, vars[idx].c_str());
return false;
}
max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) *
acceleration_scaling_factor;
max_acceleration[idx] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) *
acceleration_scaling_factor;
}
else
{
RCLCPP_ERROR_STREAM(LOGGER, "No acceleration limit was defined for joint "
<< vars[j].c_str()
<< vars[idx].c_str()
<< "! You have to define acceleration limits in the URDF or "
"joint_limits.yaml");
return false;
Expand Down Expand Up @@ -982,23 +987,30 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
double velocity_scaling_factor = verifyScalingFactor(max_velocity_scaling_factor, VELOCITY);
double acceleration_scaling_factor = verifyScalingFactor(max_acceleration_scaling_factor, ACCELERATION);

const unsigned num_joints = group->getVariableCount();
const std::vector<std::string>& vars = group->getVariableNames();
// Get the velocity and acceleration limits for specified joints
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))
{
RCLCPP_ERROR(LOGGER, "Failed to get active variable indices.");
}

const size_t num_joints = indices.size();

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

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

Expand All @@ -1008,18 +1020,18 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
if (bounds.max_velocity_ < 0.0)
{
RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0",
bounds.max_velocity_, vars[j].c_str());
bounds.max_velocity_, vars[idx].c_str());
return false;
}
max_velocity[j] =
max_velocity[idx] =
std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor;
set_velocity_limit = true;
}

if (!set_velocity_limit)
{
RCLCPP_ERROR_STREAM(LOGGER, "No velocity limit was defined for joint "
<< vars[j].c_str()
<< vars[idx].c_str()
<< "! You have to define velocity limits in the URDF or "
"joint_limits.yaml");
return false;
Expand All @@ -1028,10 +1040,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
// ACCELERATION LIMIT
// Check if a custom limit was specified as an argument
bool set_acceleration_limit = false;
it = acceleration_limits.find(vars[j]);
it = acceleration_limits.find(vars[idx]);
if (it != acceleration_limits.end())
{
max_acceleration[j] = it->second * acceleration_scaling_factor;
max_acceleration[idx] = it->second * acceleration_scaling_factor;
set_acceleration_limit = true;
}

Expand All @@ -1041,17 +1053,17 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
if (bounds.max_acceleration_ < 0.0)
{
RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0",
bounds.max_acceleration_, vars[j].c_str());
bounds.max_acceleration_, vars[idx].c_str());
return false;
}
max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) *
acceleration_scaling_factor;
max_acceleration[idx] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) *
acceleration_scaling_factor;
set_acceleration_limit = true;
}
if (!set_acceleration_limit)
{
RCLCPP_ERROR_STREAM(LOGGER, "No acceleration limit was defined for joint "
<< vars[j].c_str()
<< vars[idx].c_str()
<< "! You have to define acceleration limits in the URDF or "
"joint_limits.yaml");
return false;
Expand Down Expand Up @@ -1192,7 +1204,7 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t

bool TimeOptimalTrajectoryGeneration::hasMixedJointTypes(const moveit::core::JointModelGroup* group) const
{
const std::vector<const moveit::core::JointModel*>& joint_models = group->getJointModels();
const std::vector<const moveit::core::JointModel*>& joint_models = group->getActiveJointModels();

bool have_prismatic =
std::any_of(joint_models.cbegin(), joint_models.cend(), [](const moveit::core::JointModel* joint_model) {
Expand Down