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

Update doxygen comments for distance() and interpolate() #401

Merged
merged 3 commits into from
Mar 30, 2021
Merged

Update doxygen comments for distance() and interpolate() #401

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
dad04bd
Update doxygen comments for distance() and interpolate()
Mar 29, 2021
8ce822e
Clang format
Mar 29, 2021
a77f844
Merge branch 'main' into andyz/update_interp_comments
tylerjw Mar 29, 2021
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
10 changes: 10 additions & 0 deletions moveit_core/robot_model/include/moveit/robot_model/robot_model.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -336,7 +336,17 @@ class RobotModel
}
double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const;

/** \brief Return the sum of joint distances between two states. An L1 norm. 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
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1402,32 +1402,52 @@ class RobotState
* @{
*/

/** \brief Return the sum of joint distances to "other" state. An L1 norm. 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. An L1 norm. Only considers active joints. */
double distance(const RobotState& other, const JointModelGroup* joint_group) const;

/** \brief Return the sum of joint distances to "other" state. An L1 norm. 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