Skip to content

MTC Pick and Place tutorial uses uninitialised value for grasp_frame_transform #1079

@B-r-a-s-s

Description

@B-r-a-s-s

The tutorial "Pick and Place with MoveIt Task Constructor" nicely illustrates how to use MTC. At some point, the grasp_frame_transform is set as follows:

Eigen::Isometry3d grasp_frame_transform;
Eigen::Quaterniond q = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX()) *
                      Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) *
                      Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
grasp_frame_transform.linear() = q.matrix();
grasp_frame_transform.translation().z() = 0.1;

This worked fine for me for some time when running my own MTC task with a code block similar to the above on my device. However, suddenly when running said MTC task from a Docker container I made for it, the task consistently crashed during the planning phase. It appears this might be due to the planner using grasp_frame_transform.translation().x() and grasp_frame_transform.translation().y(), even though those are not initialised. This us fixed this by changing the above code block to the following:

Eigen::Isometry3d grasp_frame_transform = Eigen::Isometry3d::Identity();
Eigen::Quaterniond q = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX()) *
                      Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) *
                      Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
grasp_frame_transform.linear() = q.matrix();
grasp_frame_transform.translation().z() = 0.1;

I propose for the tutorial to follow this change to ensure defined behaviour. I've opened a pull request with the above proposal.

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions