From a27bd6f83558234cb1a4f787ee7e22806982237e Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Wed, 15 Mar 2023 16:18:21 -0400 Subject: [PATCH] Fix mimic joints with TOTG (#1989) --- .../moveit/robot_model/joint_model_group.h | 6 +- .../robot_model/src/joint_model_group.cpp | 26 +++---- moveit_core/robot_model/src/robot_model.cpp | 2 +- .../time_optimal_trajectory_generation.cpp | 68 +++++++++++-------- 4 files changed, 58 insertions(+), 44 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index 1b9fe18b77..65112176a4 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -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& ik_jnames, std::vector& joint_bijection) const; + /** \brief Computes the indices of joint variables given a vector of joint names to look up */ + bool computeJointVariableIndices(const std::vector& joint_names, + std::vector& 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) */ diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index f4e32c9312..a9da7f9aa6 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -138,6 +138,7 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode if (vc > 1) is_single_dof_ = false; const std::vector& name_order = joint_model->getVariableNames(); + if (joint_model->getMimic() == nullptr) { active_joint_model_vector_.push_back(joint_model); @@ -605,25 +606,25 @@ void JointModelGroup::setDefaultIKTimeout(double ik_timeout) it.second.default_ik_timeout_ = ik_timeout; } -bool JointModelGroup::computeIKIndexBijection(const std::vector& ik_jnames, - std::vector& joint_bijection) const +bool JointModelGroup::computeJointVariableIndices(const std::vector& joint_names, + std::vector& 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); } @@ -639,8 +640,8 @@ void JointModelGroup::setSolverAllocators(const std::pairsetDefaultTimeout(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(); } } @@ -655,7 +656,7 @@ void JointModelGroup::setSolverAllocators(const std::pair(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; @@ -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_) diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 30ef3db65b..609224965d 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -1550,7 +1550,7 @@ void RobotModel::setKinematicsAllocators(const std::map& 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& vars = group->getVariableNames(); + std::vector 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_) @@ -901,16 +906,16 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT 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; } @@ -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; @@ -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& vars = group->getVariableNames(); + // Get the velocity and acceleration limits for specified joints const moveit::core::RobotModel& rmodel = group->getParentModel(); + const std::vector& vars = group->getVariableNames(); + std::vector 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; } @@ -1008,10 +1020,10 @@ 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; } @@ -1019,7 +1031,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( 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; @@ -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; } @@ -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; @@ -1192,7 +1204,7 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t bool TimeOptimalTrajectoryGeneration::hasMixedJointTypes(const moveit::core::JointModelGroup* group) const { - const std::vector& joint_models = group->getJointModels(); + const std::vector& joint_models = group->getActiveJointModels(); bool have_prismatic = std::any_of(joint_models.cbegin(), joint_models.cend(), [](const moveit::core::JointModel* joint_model) {