问题
当以ros-humle-moveit应用双臂规划时,给定两个笛卡尔目标规划运行如:
move_group.setPoseTarget(another_pose_L, "L_panda_link8");
move_group.setPoseTarget(another_pose_R, "R_panda_link8");
bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
会大量重复报错,如:
[ERROR] [1646681894.535920515, 851.070000000]: IKConstraintSampler received dirty robot state, but valid transforms are required. Failing.
解决
首先我的报错为ros-humle-moveit(2.5.5版本),再此之前的moveit2可能都有此问题。幸运的是此问题已在2.6.0及以后版本修复,所以可以对比ros-jazzy-moveit(2.10.0版本)发现UnionConstraintSampler::sample的实现有变化。其他版本同理,故针对我的版本提取cpp所在的moveit_core:
cd /your/ws/path/src
git clone https://github.com/moveit/moveit2.git -b 2.5.5
mv moveit2/moveit_core moveit_core
rm -rf moveit2
cd ..
修改moveit2/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp中的UnionConstraintSampler::sample的实现为以下,其他保持不变
bool UnionConstraintSampler::sample(moveit::core::RobotState& state, const moveit::core::RobotState& reference_state,
unsigned int max_attempts)
{
state = reference_state;
for (ConstraintSamplerPtr& sampler : samplers_)
{
// ConstraintSampler::sample returns states with dirty link transforms (because it only writes values)
// but requires a state with clean link transforms as input. This means that we need to clean the link
// transforms between calls to ConstraintSampler::sample.
state.updateLinkTransforms();
if (!sampler->sample(state, max_attempts))
return false;
}
return true;
}
最后重新编译记得再. install/setup.bash一下运行即可。