Allow attached objects in move_group's /compute_ik pose frames

[ANogin]

Description

In the existing move_group implementation, the pose frame in PositionIKRequest's pose_stamped and pose_stamped_vector were converted to robot's root frame using TF2, which meant that the calls to /compute_ik service would fail with FRAME_TRANSFORM_FAILURE when one tried to use an attached object frame (or subframe) there. This changes the code to use RobotState::getFrameTransform to convert those poses to the robot's root frame instead, enabling the use of attached object frames/subframes.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[TSNoble]

Could this capability delegate purely to RobotState::setFromIK() ? As of #3077, this method should take collision objects and subframes into account.

[ANogin]

Could this capability delegate purely to RobotState::setFromIK() ? As of #3077, this method should take collision objects and subframes into account.

@TSNoble no, RobotState::setFromIK() only takes collision objects and subframes into account for the tip, but for the pose it just gets a plain pose that is interpreted against the robot root link. The frame id for the pose is never even passed into RobotState::setFromIK(). An alternative would be to further overload RobotState::setFromIK() to also take PoseStamped, not just Pose, but it's already so overloaded, that it's probably too messy.

moveit2/moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp

Lines 924 to 1026 in e7872eb

	/** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be
	set by computing inverse kinematics.
	The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
	@param pose The pose the last link in the chain needs to achieve
	@param timeout The timeout passed to the kinematics solver on each attempt
	@param constraint A state validity constraint to be required for IK solutions */
	bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, double timeout = 0.0,
	const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
	const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
	const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
	/** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be
	set by computing inverse kinematics.
	The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
	@param pose The pose the \e tip link in the chain needs to achieve
	@param tip The name of the link the pose is specified for
	@param timeout The timeout passed to the kinematics solver on each attempt
	@param constraint A state validity constraint to be required for IK solutions */
	bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, const std::string& tip,
	double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
	const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
	const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
	/** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be
	set by computing inverse kinematics.
	The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
	@param pose The pose the last link in the chain needs to achieve
	@param tip The name of the link the pose is specified for
	@param timeout The timeout passed to the kinematics solver on each attempt */
	bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0,
	const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
	const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
	const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
	/** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be
	set by computing inverse kinematics.
	The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
	@param pose The pose the last link in the chain needs to achieve
	@param timeout The timeout passed to the kinematics solver on each attempt
	@param constraint A state validity constraint to be required for IK solutions */
	bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
	double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
	const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
	const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
	/** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be
	set by computing inverse kinematics.
	The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
	@param pose The pose the last link in the chain needs to achieve
	@param tip The name of the frame for which IK is attempted.
	@param consistency_limits This specifies the desired distance between the solution and the seed state
	@param timeout The timeout passed to the kinematics solver on each attempt
	@param constraint A state validity constraint to be required for IK solutions */
	bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
	const std::vector<double>& consistency_limits, double timeout = 0.0,
	const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
	const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
	const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
	/** \brief Warning: This function inefficiently copies all transforms around.
	If the group consists of a set of sub-groups that are each a chain and a solver
	is available for each sub-group, then the joint values can be set by computing inverse kinematics.
	The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed
	to be in the same order as the order of the sub-groups in this group. Returns true on success.
	@param poses The poses the last link in each chain needs to achieve
	@param tips The names of the frames for which IK is attempted.
	@param timeout The timeout passed to the kinematics solver on each attempt
	@param constraint A state validity constraint to be required for IK solutions */
	bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
	const std::vector<std::string>& tips, double timeout = 0.0,
	const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
	const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
	const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
	/** \brief Warning: This function inefficiently copies all transforms around.
	If the group consists of a set of sub-groups that are each a chain and a solver
	is available for each sub-group, then the joint values can be set by computing inverse kinematics.
	The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed
	to be in the same order as the order of the sub-groups in this group. Returns true on success.
	@param poses The poses the last link in each chain needs to achieve
	@param tips The names of the frames for which IK is attempted.
	@param consistency_limits This specifies the desired distance between the solution and the seed state
	@param timeout The timeout passed to the kinematics solver on each attempt
	@param constraint A state validity constraint to be required for IK solutions */
	bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
	const std::vector<std::string>& tips, const std::vector<std::vector<double>>& consistency_limits,
	double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
	const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
	const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
	/**
	\brief setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solver for
	non-chain kinematics. In this case, we divide the group into subgroups and do IK solving individually
	@param poses The poses the last link in each chain needs to achieve
	@param tips The names of the frames for which IK is attempted.
	@param consistency_limits This specifies the desired distance between the solution and the seed state
	@param timeout The timeout passed to the kinematics solver on each attempt
	@param constraint A state validity constraint to be required for IK solutions */
	bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
	const std::vector<std::string>& tips,
	const std::vector<std::vector<double>>& consistency_limits, double timeout = 0.0,
	const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
	const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());

[sea-bass]

This overall looks good, but I am concerned that it breaks the behavior where the target pose does not specify a frame ID.

As seen in

moveit2/moveit_ros/move_group/src/move_group_capability.cpp

Line 203 in f82cdcd

pose_msg.header.frame_id = target_frame;

, the performTransform() function will fall back to using that default frame if the header has no frame id.

It might be beneficial to reinstate this behavior, and maybe also the previpus condition where frame ID being equal to the model frame resulting in a no-op transform.

[sea-bass]

Also, does RobotState::getFrameTransform() actually support getting frames not in the robot model, like available as external frames in the TF tree?

This may also be another major difference between the existing and proposed implementations, which would also break for any non-robot model reference frames.

[codecov-commenter]

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

Attention: Patch coverage is 0% with 12 lines in your changes missing coverage. Please review.

Project coverage is 43.94%. Comparing base (e7872eb) to head (6624b00).
Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
...ult_capabilities/kinematics_service_capability.cpp	0.00%	12 Missing ⚠️

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3144      +/-   ##
==========================================
- Coverage   46.00%   43.94%   -2.06%     
==========================================
  Files         483      697     +214     
  Lines       40632    61491   +20859     
  Branches        0     7458    +7458     
==========================================
+ Hits        18689    27014    +8325     
- Misses      21943    34309   +12366     
- Partials        0      168     +168     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.