50 , parent_link_model_(nullptr)
51 , child_link_model_(nullptr)
56 , distance_factor_(1.0)
57 , first_variable_index_(-1)
62 JointModel::~JointModel() =
default;
64 std::string JointModel::getTypeName()
const
85 int JointModel::getLocalVariableIndex(
const std::string& variable)
const
87 VariableIndexMap::const_iterator it = variable_index_map_.find(variable);
88 if (it == variable_index_map_.end())
89 throw Exception(
"Could not find variable '" + variable +
"' to get bounds for within joint '" + name_ +
"'");
93 bool JointModel::harmonizePosition(
double* values,
const Bounds& other_bounds)
const
98 bool JointModel::enforceVelocityBounds(
double* values,
const Bounds& other_bounds)
const
101 for (std::size_t i = 0; i < other_bounds.size(); ++i)
102 if (other_bounds[i].max_velocity_ < values[i])
104 values[i] = other_bounds[i].max_velocity_;
107 else if (other_bounds[i].min_velocity_ > values[i])
109 values[i] = other_bounds[i].min_velocity_;
115 bool JointModel::satisfiesVelocityBounds(
const double* values,
const Bounds& other_bounds,
double margin)
const
117 for (std::size_t i = 0; i < other_bounds.size(); ++i)
118 if (other_bounds[i].max_velocity_ + margin <
values[i])
120 else if (other_bounds[i].min_velocity_ - margin >
values[i])
125 const VariableBounds& JointModel::getVariableBounds(
const std::string& variable)
const
127 return variable_bounds_[getLocalVariableIndex(variable)];
130 void JointModel::setVariableBounds(
const std::string& variable,
const VariableBounds& bounds)
132 variable_bounds_[getLocalVariableIndex(variable)] = bounds;
133 computeVariableBoundsMsg();
136 void JointModel::setVariableBounds(
const std::vector<moveit_msgs::JointLimits>& jlim)
138 for (std::size_t j = 0; j < variable_names_.size(); ++j)
139 for (
const moveit_msgs::JointLimits& joint_limit : jlim)
140 if (joint_limit.joint_name == variable_names_[j])
142 variable_bounds_[j].position_bounded_ = joint_limit.has_position_limits;
143 if (joint_limit.has_position_limits)
145 variable_bounds_[j].min_position_ = joint_limit.min_position;
146 variable_bounds_[j].max_position_ = joint_limit.max_position;
148 variable_bounds_[j].velocity_bounded_ = joint_limit.has_velocity_limits;
149 if (joint_limit.has_velocity_limits)
151 variable_bounds_[j].min_velocity_ = -joint_limit.max_velocity;
152 variable_bounds_[j].max_velocity_ = joint_limit.max_velocity;
154 variable_bounds_[j].acceleration_bounded_ = joint_limit.has_acceleration_limits;
155 if (joint_limit.has_acceleration_limits)
157 variable_bounds_[j].min_acceleration_ = -joint_limit.max_acceleration;
158 variable_bounds_[j].max_acceleration_ = joint_limit.max_acceleration;
162 computeVariableBoundsMsg();
165 void JointModel::computeVariableBoundsMsg()
167 variable_bounds_msg_.clear();
168 for (std::size_t i = 0; i < variable_bounds_.size(); ++i)
170 moveit_msgs::JointLimits lim;
171 lim.joint_name = variable_names_[i];
172 lim.has_position_limits = variable_bounds_[i].position_bounded_;
173 lim.min_position = variable_bounds_[i].min_position_;
174 lim.max_position = variable_bounds_[i].max_position_;
175 lim.has_velocity_limits = variable_bounds_[i].velocity_bounded_;
176 lim.max_velocity = std::min(fabs(variable_bounds_[i].min_velocity_), fabs(variable_bounds_[i].max_velocity_));
177 lim.has_acceleration_limits = variable_bounds_[i].acceleration_bounded_;
178 lim.max_acceleration =
179 std::min(fabs(variable_bounds_[i].min_acceleration_), fabs(variable_bounds_[i].max_acceleration_));
180 variable_bounds_msg_.push_back(lim);
184 void JointModel::setMimic(
const JointModel* mimic,
double factor,
double offset)
187 mimic_factor_ = factor;
188 mimic_offset_ = offset;
193 mimic_requests_.push_back(joint);
196 void JointModel::addDescendantJointModel(
const JointModel* joint)
198 descendant_joint_models_.push_back(joint);
200 non_fixed_descendant_joint_models_.push_back(joint);
203 void JointModel::addDescendantLinkModel(
const LinkModel* link)
205 descendant_link_models_.push_back(link);
210 inline void printBoundHelper(std::ostream& out,
double v)
212 if (v <= -std::numeric_limits<double>::infinity())
214 else if (v >= std::numeric_limits<double>::infinity())
221 std::ostream&
operator<<(std::ostream& out,
const VariableBounds& b)
223 out <<
"P." << (b.position_bounded_ ?
"bounded" :
"unbounded") <<
" [";
224 printBoundHelper(out, b.min_position_);
226 printBoundHelper(out, b.max_position_);
228 <<
"V." << (b.velocity_bounded_ ?
"bounded" :
"unbounded") <<
" [";
229 printBoundHelper(out, b.min_velocity_);
231 printBoundHelper(out, b.max_velocity_);
233 <<
"A." << (b.acceleration_bounded_ ?
"bounded" :
"unbounded") <<
" [";
234 printBoundHelper(out, b.min_acceleration_);
236 printBoundHelper(out, b.max_acceleration_);