Skip to content

Commit

Permalink
Update doxygen comments for distance() and interpolate() (moveit#2528)
Browse files Browse the repository at this point in the history
  • Loading branch information
AndyZe authored and tylerjw committed Apr 29, 2021
1 parent bf423c5 commit b946c38
Show file tree
Hide file tree
Showing 2 changed files with 39 additions and 9 deletions.
10 changes: 10 additions & 0 deletions moveit_core/robot_model/include/moveit/robot_model/robot_model.h
Original file line number Diff line number Diff line change
Expand Up @@ -337,7 +337,17 @@ class RobotModel
}
double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const;

/** \brief Return the sum of joint distances between two states. Only considers active joints. */
double distance(const double* state1, const double* state2) const;

/**
* Interpolate between "from" state, to "to" state. Mimic joints are correctly updated.
*
* @param from interpolate from this state
* @param to to this state
* @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly.
* @param state holds the result
*/
void interpolate(const double* from, const double* to, double t, double* state) const;

/** \name Access to joint groups
Expand Down
38 changes: 29 additions & 9 deletions moveit_core/robot_state/include/moveit/robot_state/robot_state.h
Original file line number Diff line number Diff line change
Expand Up @@ -1521,32 +1521,52 @@ as the new values that correspond to the group */
* @{
*/

/** \brief Return the sum of joint distances to "other" state. Only considers active joints. */
double distance(const RobotState& other) const
{
return robot_model_->distance(position_, other.getVariablePositions());
}

/** \brief Return the sum of joint distances to "other" state. Only considers active joints. */
double distance(const RobotState& other, const JointModelGroup* joint_group) const;

/** \brief Return the sum of joint distances to "other" state. Only considers active joints. */
double distance(const RobotState& other, const JointModel* joint) const
{
const int idx = joint->getFirstVariableIndex();
return joint->distance(position_ + idx, other.position_ + idx);
}

/** \brief Interpolate from this state towards state \e to, at time \e t in [0,1].
The result is stored in \e state, mimic joints are correctly updated and flags are set
so that FK is recomputed when needed. */
/**
* Interpolate towards "to" state. Mimic joints are correctly updated and flags are set so that FK is recomputed
* when needed.
*
* @param to interpolate to this state
* @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly.
* @param state holds the result
*/
void interpolate(const RobotState& to, double t, RobotState& state) const;

/** \brief Interpolate from this state towards \e to, at time \e t in [0,1], but only for the joints in the
specified group. If mimic joints need to be updated, they are updated accordingly. Flags are set so that FK
computation is triggered when needed. */
/**
* Interpolate towards "to" state, but only for the joints in the specified group. Mimic joints are correctly updated
* and flags are set so that FK is recomputed when needed.
*
* @param to interpolate to this state
* @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly.
* @param state holds the result
* @param joint_group interpolate only for the joints in this group
*/
void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;

/** \brief Update \e state by interpolating form this state towards \e to, at time \e t in [0,1] but only for
the joint \e joint. If there are joints that mimic this joint, they are updated. Flags are set so that
FK computation is triggered as needed. */
/**
* Interpolate towards "to" state, but only for a single joint. Mimic joints are correctly updated
* and flags are set so that FK is recomputed when needed.
*
* @param to interpolate to this state
* @param t a fraction in the range [0 1]. If 1, the result matches "to" state exactly.
* @param state holds the result
* @param joint interpolate only for this joint
*/
void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
{
const int idx = joint->getFirstVariableIndex();
Expand Down

0 comments on commit b946c38

Please sign in to comment.