26 #include <VirtualRobot/math/Helpers.h>
34 std::vector<Eigen::Matrix4f>
RobotPlacement::CreateGrid(
float dx,
int minx,
int maxx,
float dy,
int miny,
int maxy,
float da,
int mina,
int maxa)
36 std::vector<Eigen::Matrix4f> r;
37 for (
int x = minx; x <= maxx; x++)
38 for (
int y = miny; y <= maxy; y++)
39 for (
int a = mina;
a <= maxa;
a++)
41 r.emplace_back(math::Helpers::CreatePose(Eigen::Vector3f(x * dx, y * dy, 0), Eigen::AngleAxisf(
a * da, Eigen::Vector3f::UnitZ()).toRotationMatrix()));
48 std::vector<RobotPlacement::Result> r;
49 std::vector<Eigen::Matrix4f> tcpTargets;
52 tcpTargets.emplace_back(pp);
54 std::vector<Eigen::Matrix4f> grasPoses = params.
graspTrajectory->getAllKeypointPoses();
59 std::vector<Eigen::Matrix4f> localPoses;
62 localPoses.emplace_back(invPlacement * tcpPose);
64 DiffIKResult ikResult = ikProvider->SolveAbsolute(localPoses.at(0));
65 throw LocalException(
"not implemented");