Planning failure with path constraints

[marip8]

Using this plugin as the IK solver during OMPL planning with path constraints (using RRT connect) results in the following assertion error:

[ WARN] [1597930803.124346496]: Returning dirty link transforms
move_group: /home/mripperger/ros/moveit_ws/src/moveit/moveit_core/robot_state/src/robot_state.cpp:979: const Affine3d& moveit::core::RobotState::getFrameTransform(const string&) const: Assertion `checkLinkTransforms()' failed.

I experience this issue when building MoveIt from source at 0.9.18 on Xenial/Kinetic, specifically using a single orientation path constraint. I still need to confirm if this happens on the distributed versions on Kinetic/Melodic/Noetic.

This error does not occur during planning without path constraints

Planning with the joint space state model

The culprit appears to be the ASSUMED_FIXED_ROOT_JOINT

Debug variable state at assertion failure

Locals		
  joint_position	-0.96499999999999897	double
  joint_vals	<7 items>	std::vector<double, std::allocator<double> >
      -0.964999999999999	double
      0	double
      0	double
      0	double
      0	double
      0	double
      0	double
  this	@0x10df540	sampling_kinematics_plugin::ExternalAxisSamplingKinematicsPlugin
    [kinematics::KinematicsBase]	@0x10df540	kinematics::KinematicsBase
    cost_weights_	<7 items>	std::vector<double, std::allocator<double> >
    ik_group_info_	@0x10df670	moveit_msgs::KinematicSolverInfo
    joint_model_group_	@0x5666190	moveit::core::JointModelGroup
    loader_	@0x10df6e0	pluginlib::ClassLoader<kinematics::KinematicsBase>
    robot_base_frame_	"robot_base_link"	std::__cxx11::string
    robot_model_	@0x5591970	moveit::core::RobotModelConstPtr
    robot_state_	@0x57fdb10	moveit::core::RobotStatePtr
      acceleration_	0	double
      attached_body_map_	<0 items>	std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, moveit::core::AttachedBody*, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, moveit::core::AttachedBody*> > >
      attached_body_update_callback_	@0x57fdbb0	moveit::core::AttachedBodyCallback
      dirty_collision_body_transforms_	@0x5405470	moveit::core::FixedJointModel
      dirty_joint_transforms_	"\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001\001"	unsigned char*
      dirty_link_transforms_	@0x5405470	moveit::core::FixedJointModel
        [moveit::core::JointModel]	@0x5405470	moveit::core::JointModel
          [vptr]	_vptr.JointModel	 
          child_link_model_	@0x5591d30	moveit::core::LinkModel
            associated_fixed_transforms_	<16 items>	moveit::core::LinkTransformMap
            centered_bounding_box_offset_	(3 x 1), ColumnMajor	Eigen::Vector3d
            child_joint_models_	<3 items>	std::vector<moveit::core::JointModel const*, std::allocator<moveit::core::JointModel const*> >
            collision_origin_transform_	<1 items>	EigenSTL::vector_Affine3d
            collision_origin_transform_is_identity_	<1 items>	std::vector<int, std::allocator<int> >
            first_collision_body_transform_index_	0	int
            is_parent_joint_fixed_	true	bool
            joint_origin_transform_	@0x5591d80	Eigen::Affine3d
            joint_origin_transform_is_identity_	true	bool
            link_index_	0	int
            name_	"base_link"	std::__cxx11::string
            parent_joint_model_	@0x5405470	moveit::core::FixedJointModel
            parent_link_model_	0x0	moveit::core::LinkModel*
            shape_extents_	(3 x 1), ColumnMajor	Eigen::Vector3d
            shapes_	<1 items>	std::vector<std::shared_ptr<shapes::Shape const>, std::allocator<std::shared_ptr<shapes::Shape const> > >
            visual_mesh_filename_	""	std::__cxx11::string
            visual_mesh_origin_	@0x5591ed0	Eigen::Affine3d
            visual_mesh_scale_	(3 x 1), ColumnMajor	Eigen::Vector3d
          descendant_joint_models_	<54 items>	std::vector<moveit::core::JointModel const*, std::allocator<moveit::core::JointModel const*> >
          descendant_link_models_	<55 items>	std::vector<moveit::core::LinkModel const*, std::allocator<moveit::core::LinkModel const*> >
          distance_factor_	0	double
          first_variable_index_	-1	int
          joint_index_	0	int
          local_variable_names_	<0 items>	std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >
          mimic_	0x0	moveit::core::JointModel*
          mimic_factor_	1	double
          mimic_offset_	0	double
          mimic_requests_	<0 items>	std::vector<moveit::core::JointModel const*, std::allocator<moveit::core::JointModel const*> >
          name_	"ASSUMED_FIXED_ROOT_JOINT"	std::__cxx11::string
          non_fixed_descendant_joint_models_	<9 items>	std::vector<moveit::core::JointModel const*, std::allocator<moveit::core::JointModel const*> >
          parent_link_model_	0x0	moveit::core::LinkModel*
          passive_	false	bool
          type_	moveit::core::JointModel::FIXED (5)	moveit::core::JointModel::JointType
          variable_bounds_	<0 items>	moveit::core::JointModel::Bounds
          variable_bounds_msg_	<0 items>	std::vector<moveit_msgs::JointLimits_<std::allocator<void> >, std::allocator<moveit_msgs::JointLimits_<std::allocator<void> > > >
          variable_index_map_	<0 items>	moveit::core::VariableIndexMap
          variable_names_	<0 items>	std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >
      effort_	0	double
      global_collision_body_transforms_	@0x540c2d0	Eigen::Affine3d
      global_link_transforms_	@0x540a750	Eigen::Affine3d
      has_acceleration_	false	bool
      has_effort_	false	bool
      has_velocity_	false	bool
      memory_	0x5408bd0	void*
      position_	0	double
      rng_	0x0	random_numbers::RandomNumberGenerator*
      robot_model_	@0x5591970	moveit::core::RobotModelConstPtr
      variable_joint_transforms_	@0x5408bd0	Eigen::Affine3d
      velocity_	0	double
    solver_	@0x540dd60	kinematics::KinematicsBasePtr
Inspector		
Expressions		
Return Value		
Tooltip		

Planning with cartesian space state model

The culprit seems to be the ASSUMED_FIXED_ROOT_JOINT (as revealed by a print statement I added in the MoveIt code), but the debugger shows that the robot_state_ object doesn't actually have any dirty link transforms

Debug variable state at assertion failure

Locals		
  joint_position	-1.5649999999999995	double
  joint_vals	<7 items>	std::vector<double, std::allocator<double> >
  this	@0x2199510	sampling_kinematics_plugin::ExternalAxisSamplingKinematicsPlugin
    [kinematics::KinematicsBase]	@0x2199510	kinematics::KinematicsBase
    cost_weights_	<7 items>	std::vector<double, std::allocator<double> >
    ik_group_info_	@0x2199640	moveit_msgs::KinematicSolverInfo
    joint_model_group_	@0x662bd80	moveit::core::JointModelGroup
    loader_	@0x21996b0	pluginlib::ClassLoader<kinematics::KinematicsBase>
    robot_base_frame_	"robot_base_link"	std::__cxx11::string
    robot_model_	@0x664e800	moveit::core::RobotModelConstPtr
    robot_state_	@0x673e6c0	moveit::core::RobotStatePtr
      acceleration_	0	double
      attached_body_map_	<0 items>	std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, moveit::core::AttachedBody*, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, moveit::core::AttachedBody*> > >
      attached_body_update_callback_	@0x673e760	moveit::core::AttachedBodyCallback
      dirty_collision_body_transforms_	@0x219d210	moveit::core::FixedJointModel
      dirty_joint_transforms_	""	unsigned char*
      dirty_link_transforms_	0x0	moveit::core::JointModel*
      effort_	0	double
      global_collision_body_transforms_	@0x6632b80	Eigen::Affine3d
      global_link_transforms_	@0x6631000	Eigen::Affine3d
      has_acceleration_	false	bool
      has_effort_	false	bool
      has_velocity_	false	bool
      memory_	0x662f480	void*
      position_	0	double
      rng_	0x0	random_numbers::RandomNumberGenerator*
      robot_model_	@0x664e800	moveit::core::RobotModelConstPtr
      variable_joint_transforms_	@0x662f480	Eigen::Affine3d
      velocity_	0	double
    solver_	@0x673f1b0	kinematics::KinematicsBasePtr
Inspector		
Expressions		
Return Value		
Tooltip	

Possible Solutions

This seems to be the same or a similar issue to this one which was apparently already addressed by a couple of pull requests.

I've also tried adding calls to RobotState::update and RobotState::updateLinkTransforms after calling setJointGroupPositions below to no avail

sampling_kinematics_plugin/sampling_kinematics_plugin/src/external_axis_sampling_kinematics_plugin.cpp

Line 212 in af054c3

robot_state_->setJointGroupPositions(group_name_, joint_vals);