Skip to content

Commit

Permalink
Fix mimic joints with TOTG (moveit#1989)
Browse files Browse the repository at this point in the history
  • Loading branch information
@sea-bass
sea-bass authored Mar 15, 2023
1 parent e02bd19 commit a27bd6f
Show file tree
Hide file tree
Showing 4 changed files with 58 additions and 44 deletions.
6 changes: 4 additions & 2 deletions moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h
View file
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
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
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
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
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

0 comments on commit a27bd6f

Please sign in to comment.