RobotPlacement.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
5  * Karlsruhe Institute of Technology (KIT), all rights reserved.
6  *
7  * ArmarX is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License version 2 as
9  * published by the Free Software Foundation.
10  *
11  * ArmarX is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program. If not, see <http://www.gnu.org/licenses/>.
18  *
19  * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
20  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
21  * GNU General Public License
22  */
23 
24 #include "RobotPlacement.h"
25 
26 #include <VirtualRobot/math/Helpers.h>
27 
28 using namespace armarx;
29 
31 {
32 }
33 
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)
35 {
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++)
40  {
41  r.emplace_back(math::Helpers::CreatePose(Eigen::Vector3f(x * dx, y * dy, 0), Eigen::AngleAxisf(a * da, Eigen::Vector3f::UnitZ()).toRotationMatrix()));
42  }
43  return r;
44 }
45 
46 std::vector<RobotPlacement::Result> RobotPlacement::evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements, const RobotPlacement::PlacementParams& params)
47 {
48  std::vector<RobotPlacement::Result> r;
49  std::vector<Eigen::Matrix4f> tcpTargets;
50  for (const Eigen::Matrix4f& pp : params.prePoses)
51  {
52  tcpTargets.emplace_back(pp);
53  }
54  std::vector<Eigen::Matrix4f> grasPoses = params.graspTrajectory->getAllKeypointPoses();
55 
56  for (const Eigen::Matrix4f& placement : robotPlacements)
57  {
58  Eigen::Matrix4f invPlacement = placement.inverse();
59  std::vector<Eigen::Matrix4f> localPoses;
60  for (const Eigen::Matrix4f& tcpPose : tcpTargets)
61  {
62  localPoses.emplace_back(invPlacement * tcpPose);
63  }
64  DiffIKResult ikResult = ikProvider->SolveAbsolute(localPoses.at(0));
65  throw LocalException("not implemented");
66  }
67  return {};
68 }
armarx::DiffIKResult
Definition: DiffIKProvider.h:34
armarx::RobotPlacement::PlacementParams
Definition: RobotPlacement.h:42
armarx::DiffIKProviderPtr
std::shared_ptr< class DiffIKProvider > DiffIKProviderPtr
Definition: DiffIKProvider.h:32
armarx::RobotPlacement::RobotPlacement
RobotPlacement(const DiffIKProviderPtr &ikProvider)
Definition: RobotPlacement.cpp:30
armarx::RobotPlacement::evaluatePlacements
std::vector< Result > evaluatePlacements(const std::vector< Eigen::Matrix4f > &robotPlacements, const PlacementParams &params)
Definition: RobotPlacement.cpp:46
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
armarx::RobotPlacement::PlacementParams::prePoses
std::vector< Eigen::Matrix4f > prePoses
Definition: RobotPlacement.h:44
armarx::RobotPlacement::PlacementParams::graspTrajectory
GraspTrajectoryPtr graspTrajectory
Definition: RobotPlacement.h:45
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
RobotPlacement.h
armarx::RobotPlacement::CreateGrid
static std::vector< Eigen::Matrix4f > CreateGrid(float dx, int minx, int maxx, float dy, int miny, int maxy, float da, int mina, int maxa)
Definition: RobotPlacement.cpp:34
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28