mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveRelative: possibly update last waypoint before transform lookup
Apparently this RobotState is not necessarily updated, this broke some pipeline testing over here.
This commit is contained in:
parent
38cc4c6936
commit
e5fc8b62b4
@ -235,8 +235,9 @@ COMPUTE:
|
||||
if (min_distance > 0.0) {
|
||||
double distance = 0.0;
|
||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
|
||||
const robot_state::RobotState& reached_state = robot_trajectory->getLastWayPoint();
|
||||
Eigen::Affine3d reached_pose = reached_state.getGlobalLinkTransform(link);
|
||||
robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
||||
reached_state->updateLinkTransforms();
|
||||
const Eigen::Affine3d& reached_pose = reached_state->getGlobalLinkTransform(link);
|
||||
if (use_rotation_distance) {
|
||||
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
|
||||
distance = rotation.angle();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user