Skip to content

Commit

Permalink
Make setToIKSolverFrame accessible again (moveit#2580)
Browse files Browse the repository at this point in the history
This is public API because the solver's getPositionIK requires input poses
in the frame and the relevant transform is otherwise inaccessible.

Partially reverts moveit#2461.
  • Loading branch information
v4hn authored and tylerjw committed Apr 29, 2021
1 parent 6316cd6 commit 5a4aa07
Showing 1 changed file with 6 additions and 6 deletions.
12 changes: 6 additions & 6 deletions moveit_core/robot_state/include/moveit/robot_state/robot_state.h
Original file line number Diff line number Diff line change
Expand Up @@ -1831,18 +1831,13 @@ as the new values that correspond to the group */

std::string getStateTreeString(const std::string& prefix = "") const;

private:
void allocMemory();
void initTransforms();
void copyFrom(const RobotState& other);

/**
* \brief Transform pose from the robot model's base frame to the reference frame of the IK solver
* @param pose - the input to change
* @param solver - a kin solver whose base frame is important to us
* @return true if no error
*/
inline bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver);

/**
* \brief Transform pose from the robot model's base frame to the reference frame of the IK solver
Expand All @@ -1852,6 +1847,11 @@ as the new values that correspond to the group */
*/
bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame);

private:
void allocMemory();
void initTransforms();
void copyFrom(const RobotState& other);

void markDirtyJointTransforms(const JointModel* joint)
{
dirty_joint_transforms_[joint->getJointIndex()] = 1;
Expand Down

0 comments on commit 5a4aa07

Please sign in to comment.