SimpleDiffIK.h
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 #pragma once
25 
26 #include "DiffIKProvider.h"
27 
28 #include <VirtualRobot/Nodes/RobotNode.h>
29 #include <VirtualRobot/RobotNodeSet.h>
30 
31 #include <memory>
32 
33 namespace armarx
34 {
35  using SimpleDiffIKPtr = std::shared_ptr<class SimpleDiffIK>;
36 
38  {
39  public:
40  struct Parameters
41  {
43  // IK params
44  float ikStepLengthInitial = 0.2f;
45  float ikStepLengthFineTune = 0.5f;
46  size_t stepsInitial = 25;
47  size_t stepsFineTune = 10;
48  float maxPosError = 10.f;
49  float maxOriError = 0.05f;
50  float jointLimitAvoidanceKp = 2.0f;
51  float maxJointAngleStep = 0.1f;
52  bool returnIKSteps = false;
53  bool resetRnsValues = true;
54  };
55  struct IKStep
56  {
57  Eigen::VectorXf jointValues;
58  Eigen::Vector3f posDiff;
59  Eigen::Vector3f oriDiff;
60  Eigen::VectorXf cartesianVel;
61  Eigen::VectorXf jnv;
62  Eigen::VectorXf jv;
63  float infNorm;
64  Eigen::VectorXf jvClamped;
65  };
66 
67  struct Result
68  {
69  Eigen::VectorXf jointValues;
70  Eigen::Vector3f posDiff;
71  Eigen::Vector3f oriDiff;
72  float posError;
73  float oriError;
74  bool reached;
75  Eigen::VectorXf jointLimitMargins;
77  std::vector<IKStep> ikSteps;
78  };
79 
80 
81  struct Reachability
82  {
83 
84  bool reachable = true;
86  Eigen::VectorXf jointLimitMargins;
87  float maxPosError = 0;
88  float maxOriError = 0;
89  std::vector<Result> ikResults;
90 
91  void aggregate(const Result& result)
92  {
93  ikResults.emplace_back(result);
94  reachable = reachable && result.reached;
96  if (jointLimitMargins.rows() == 0)
97  {
99  }
100  else
101  {
103  }
106  }
107  };
108 
109  static Result CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
110 
111  ///@brief Use this to check a trajectory of waypoints
112  static Reachability CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
113  };
114 
116  public DiffIKProvider
117  {
118  public:
119  SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
120  DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose);
121  DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues);
122 
123  private:
124  VirtualRobot::RobotNodeSetPtr rns;
125  VirtualRobot::RobotNodePtr tcp;
127  };
128 }
armarx::SimpleDiffIK::Result::posDiff
Eigen::Vector3f posDiff
Definition: SimpleDiffIK.h:70
armarx::SimpleDiffIK::Parameters::Parameters
Parameters()
Definition: SimpleDiffIK.h:42
armarx::SimpleDiffIK::IKStep::jvClamped
Eigen::VectorXf jvClamped
Definition: SimpleDiffIK.h:64
armarx::SimpleDiffIK::Parameters::ikStepLengthFineTune
float ikStepLengthFineTune
Definition: SimpleDiffIK.h:45
armarx::SimpleDiffIK::Parameters::stepsInitial
size_t stepsInitial
Definition: SimpleDiffIK.h:46
armarx::SimpleDiffIK::IKStep::cartesianVel
Eigen::VectorXf cartesianVel
Definition: SimpleDiffIK.h:60
armarx::SimpleDiffIKProvider
Definition: SimpleDiffIK.h:115
armarx::SimpleDiffIK::IKStep::posDiff
Eigen::Vector3f posDiff
Definition: SimpleDiffIK.h:58
armarx::SimpleDiffIK::Parameters::returnIKSteps
bool returnIKSteps
Definition: SimpleDiffIK.h:52
armarx::DiffIKResult
Definition: DiffIKProvider.h:34
armarx::SimpleDiffIK::Reachability::aggregate
void aggregate(const Result &result)
Definition: SimpleDiffIK.h:91
armarx::SimpleDiffIK::Parameters
Definition: SimpleDiffIK.h:40
armarx::SimpleDiffIK::Reachability::reachable
bool reachable
Definition: SimpleDiffIK.h:84
armarx::SimpleDiffIK::Result::reached
bool reached
Definition: SimpleDiffIK.h:74
armarx::SimpleDiffIK::Reachability::minimumJointLimitMargin
float minimumJointLimitMargin
Definition: SimpleDiffIK.h:85
armarx::SimpleDiffIK
Definition: SimpleDiffIK.h:37
armarx::SimpleDiffIK::CalculateDiffIK
static Result CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp=VirtualRobot::RobotNodePtr(), Parameters params=Parameters())
Definition: SimpleDiffIK.cpp:33
armarx::SimpleDiffIK::Reachability
Definition: SimpleDiffIK.h:81
DiffIKProvider.h
armarx::SimpleDiffIK::IKStep::infNorm
float infNorm
Definition: SimpleDiffIK.h:63
armarx::SimpleDiffIK::Result::minimumJointLimitMargin
float minimumJointLimitMargin
Definition: SimpleDiffIK.h:76
armarx::SimpleDiffIK::IKStep::oriDiff
Eigen::Vector3f oriDiff
Definition: SimpleDiffIK.h:59
armarx::SimpleDiffIK::IKStep::jv
Eigen::VectorXf jv
Definition: SimpleDiffIK.h:62
armarx::SimpleDiffIK::Result::jointLimitMargins
Eigen::VectorXf jointLimitMargins
Definition: SimpleDiffIK.h:75
armarx::SimpleDiffIK::Result::jointValues
Eigen::VectorXf jointValues
Definition: SimpleDiffIK.h:69
armarx::SimpleDiffIK::Parameters::jointLimitAvoidanceKp
float jointLimitAvoidanceKp
Definition: SimpleDiffIK.h:50
armarx::SimpleDiffIK::Parameters::maxOriError
float maxOriError
Definition: SimpleDiffIK.h:49
armarx::SimpleDiffIKProvider::SolveRelative
DiffIKResult SolveRelative(const Eigen::Matrix4f &targetPose, const Eigen::VectorXf &startJointValues)
Definition: SimpleDiffIK.cpp:161
armarx::SimpleDiffIK::Result::posError
float posError
Definition: SimpleDiffIK.h:72
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::SimpleDiffIK::IKStep::jointValues
Eigen::VectorXf jointValues
Definition: SimpleDiffIK.h:57
armarx::SimpleDiffIK::Parameters::maxJointAngleStep
float maxJointAngleStep
Definition: SimpleDiffIK.h:51
armarx::SimpleDiffIKProvider::SimpleDiffIKProvider
SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp=VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params=SimpleDiffIK::Parameters())
Definition: SimpleDiffIK.cpp:143
max
T max(T t1, T t2)
Definition: gdiam.h:48
armarx::SimpleDiffIK::Parameters::ikStepLengthInitial
float ikStepLengthInitial
Definition: SimpleDiffIK.h:44
armarx::SimpleDiffIK::IKStep::jnv
Eigen::VectorXf jnv
Definition: SimpleDiffIK.h:61
armarx::SimpleDiffIK::Reachability::jointLimitMargins
Eigen::VectorXf jointLimitMargins
Definition: SimpleDiffIK.h:86
armarx::SimpleDiffIK::Result::oriError
float oriError
Definition: SimpleDiffIK.h:73
armarx::SimpleDiffIK::Reachability::maxOriError
float maxOriError
Definition: SimpleDiffIK.h:88
armarx::SimpleDiffIK::Result::ikSteps
std::vector< IKStep > ikSteps
Definition: SimpleDiffIK.h:77
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::SimpleDiffIK::Result
Definition: SimpleDiffIK.h:67
armarx::SimpleDiffIKPtr
std::shared_ptr< class SimpleDiffIK > SimpleDiffIKPtr
Definition: SimpleDiffIK.h:35
armarx::SimpleDiffIK::CalculateReachability
static Reachability CalculateReachability(const std::vector< Eigen::Matrix4f > targets, const Eigen::VectorXf &initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp=VirtualRobot::RobotNodePtr(), Parameters params=Parameters())
Use this to check a trajectory of waypoints.
Definition: SimpleDiffIK.cpp:127
armarx::SimpleDiffIK::Parameters::maxPosError
float maxPosError
Definition: SimpleDiffIK.h:48
armarx::SimpleDiffIK::Result::oriDiff
Eigen::Vector3f oriDiff
Definition: SimpleDiffIK.h:71
armarx::SimpleDiffIKProvider::SolveAbsolute
DiffIKResult SolveAbsolute(const Eigen::Matrix4f &targetPose)
Definition: SimpleDiffIK.cpp:148
armarx::SimpleDiffIK::IKStep
Definition: SimpleDiffIK.h:55
min
T min(T t1, T t2)
Definition: gdiam.h:42
armarx::SimpleDiffIK::Reachability::ikResults
std::vector< Result > ikResults
Definition: SimpleDiffIK.h:89
armarx::SimpleDiffIK::Parameters::stepsFineTune
size_t stepsFineTune
Definition: SimpleDiffIK.h:47
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::SimpleDiffIK::Parameters::resetRnsValues
bool resetRnsValues
Definition: SimpleDiffIK.h:53
armarx::SimpleDiffIK::Reachability::maxPosError
float maxPosError
Definition: SimpleDiffIK.h:87
armarx::DiffIKProvider
Definition: DiffIKProvider.h:43