Planning failure with path constraints #11
Description
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