NaturalIK.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 <memory>
27 
28 //#include <RobotAPI/libraries/core/SimpleDiffIK.h>
29 #include <VirtualRobot/Nodes/RobotNode.h>
32 #include <optional>
33 
34 namespace armarx
35 {
36  using NaturalIKPtr = std::shared_ptr<class NaturalIK>;
37 
38  /**
39  * @brief The NaturalIK class
40  *
41  * Calculates natural looking IK solutions by
42  * 1. Calculate Soechting angles
43  * 2. Calculate ForwardKinematics with Soechting angles
44  * 3. Use calculated elbow position as a nullspace target for the robot's elbow during diffik
45  *
46  * WARNING: Only works if robot model is facing forward in Y-direction (like ARMAR-III, ARMAR-IV, ARMAR-6)
47  * WARNING2: Soechting model is not valid for all targets. However, elbow position seems to extrapolate fine.
48  */
49  class NaturalIK
50  {
51  public:
52  struct Parameters
53  {
56  std::optional<float> minimumElbowHeight = std::nullopt;
57 
58  };
59  struct ArmJoints
60  {
61  VirtualRobot::RobotNodeSetPtr rns;
62  VirtualRobot::RobotNodePtr elbow;
63  VirtualRobot::RobotNodePtr shoulder;
64  VirtualRobot::RobotNodePtr tcp;
65  };
66 
67 
69  {
70  float SE;
71  float SY;
72  float EE;
73  float EY;
74 
75 
76  static constexpr float MMM_LS2LE = 0.188;
77  static constexpr float MMM_LE2LW = 0.145;
78  static constexpr float MMM_L = 1700;
79  static constexpr float MMM_UPPER_ARM_LENGTH = MMM_LS2LE * MMM_L;
80  static constexpr float MMM_LOWER_ARM_LENGTH = MMM_LE2LW * MMM_L;
81 
82  static constexpr float MMM_LSC2LS = 0.023;
83  static constexpr float MMM_BT2LSC = 0.087;
84 
85  static constexpr float MMM_CLAVICULAR_WIDTH = MMM_BT2LSC * MMM_L;
86  static constexpr float MMM_SHOULDER_POS = (MMM_LSC2LS + MMM_BT2LSC) * MMM_L;
87  };
88 
90  {
91  Eigen::Vector3f shoulder;
92  Eigen::Vector3f elbow;
93  Eigen::Vector3f wrist;
95  };
96 
97  public:
98  NaturalIK(std::string side, Eigen::Vector3f shoulderPos = Eigen::Vector3f::Zero(), float scale = 1);
99 
100  NaturalIK::SoechtingForwardPositions solveSoechtingIK(const Eigen::Vector3f& targetPos, std::optional<float> minElbowHeight = std::nullopt);
102  NaturalDiffIK::Result calculateIKpos(const Eigen::Vector3f& targetPos, ArmJoints arm, NaturalIK::Parameters params = NaturalIK::Parameters());
103 
105 
106  //static SimpleDiffIK::Result CalculateNaturalIK(const Eigen::Matrix4f targetPose, ArmJoints armjoints, Parameters params = Parameters());
107 
108  Eigen::Vector3f getShoulderPos();
109 
110  /**
111  * @brief NaturalIK::CalculateSoechtingAngles
112  * @param target Pointing target in mm, relative to the soulder position. X: right, Y: forward, Z: up
113  * @param side: Left or Right
114  * @param scale: scale factor to match the robots arm length to the human arm length
115  * @return Angles described in Soechting and Flanders "Errors in Pointing are Due to Approximations in Sensorimotor Transformations"
116  */
118 
120 
121  void setScale(float scale);
122  float getScale();
123 
124 
125  float getUpperArmLength() const;
126  void setUpperArmLength(float value);
127 
128  float getLowerArmLength() const;
129  void setLowerArmLength(float value);
130 
131  private:
132  std::string side;
133  Eigen::Vector3f shoulderPos;
134  float scale;
135 
136  int soechtingIterations = 2;
137 
138  float upperArmLength = SoechtingAngles::MMM_UPPER_ARM_LENGTH;
139  float lowerArmLength = SoechtingAngles::MMM_LOWER_ARM_LENGTH;
140  };
141 
143  : public DiffIKProvider
144  {
145  public:
147  DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose);
148  DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues);
149 
150  private:
153  NaturalDiffIK::Mode setOri;
154  NaturalIK::Parameters params;
155  };
156 }
NaturalDiffIK.h
armarx::NaturalIK::Parameters
Definition: NaturalIK.h:52
armarx::NaturalIK::SoechtingAngles
Definition: NaturalIK.h:68
armarx::DiffIKResult
Definition: DiffIKProvider.h:34
armarx::natik
Brief description of class natik.
Definition: natik.h:39
armarx::NaturalIK::SoechtingAngles::MMM_SHOULDER_POS
static constexpr float MMM_SHOULDER_POS
Definition: NaturalIK.h:86
armarx::NaturalIK::calculateIK
NaturalDiffIK::Result calculateIK(const Eigen::Matrix4f &targetPose, ArmJoints arm, NaturalIK::Parameters params=NaturalIK::Parameters())
Definition: NaturalIK.cpp:84
armarx::NaturalIK::ArmJoints::shoulder
VirtualRobot::RobotNodePtr shoulder
Definition: NaturalIK.h:63
armarx::NaturalIK::ArmJoints::tcp
VirtualRobot::RobotNodePtr tcp
Definition: NaturalIK.h:64
armarx::NaturalIK::SoechtingForwardPositions::wrist
Eigen::Vector3f wrist
Definition: NaturalIK.h:93
armarx::NaturalIK::SoechtingAngles::MMM_LOWER_ARM_LENGTH
static constexpr float MMM_LOWER_ARM_LENGTH
Definition: NaturalIK.h:80
armarx::NaturalDiffIK::Mode
Mode
Definition: NaturalDiffIK.h:36
armarx::NaturalIK::ArmJoints::elbow
VirtualRobot::RobotNodePtr elbow
Definition: NaturalIK.h:62
armarx::NaturalIK::SoechtingAngles::EE
float EE
Definition: NaturalIK.h:72
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:688
armarx::NaturalIK::SoechtingAngles::MMM_L
static constexpr float MMM_L
Definition: NaturalIK.h:78
armarx::NaturalIK::CalculateSoechtingAngles
SoechtingAngles CalculateSoechtingAngles(Eigen::Vector3f target)
NaturalIK::CalculateSoechtingAngles.
Definition: NaturalIK.cpp:112
DiffIKProvider.h
armarx::NaturalIK::SoechtingAngles::MMM_LE2LW
static constexpr float MMM_LE2LW
Definition: NaturalIK.h:77
armarx::NaturalIK::ArmJoints
Definition: NaturalIK.h:59
armarx::NaturalIK::SoechtingAngles::MMM_LSC2LS
static constexpr float MMM_LSC2LS
Definition: NaturalIK.h:82
armarx::NaturalIK::getScale
float getScale()
Definition: NaturalIK.cpp:194
armarx::NaturalIK::SoechtingAngles::SY
float SY
Definition: NaturalIK.h:71
armarx::NaturalIK::setLowerArmLength
void setLowerArmLength(float value)
Definition: NaturalIK.cpp:214
armarx::NaturalIKPtr
std::shared_ptr< class NaturalIK > NaturalIKPtr
Definition: NaturalIK.h:36
armarx::NaturalIK::Parameters::Parameters
Parameters()
Definition: NaturalIK.h:54
armarx::NaturalIK::NaturalIK
NaturalIK(std::string side, Eigen::Vector3f shoulderPos=Eigen::Vector3f::Zero(), float scale=1)
Definition: NaturalIK.cpp:33
armarx::NaturalIK
The NaturalIK class.
Definition: NaturalIK.h:49
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
armarx::NaturalIK::setUpperArmLength
void setUpperArmLength(float value)
Definition: NaturalIK.cpp:204
armarx::NaturalIKProvider::NaturalIKProvider
NaturalIKProvider(const NaturalIK &natik, const NaturalIK::ArmJoints &arm, const NaturalDiffIK::Mode &setOri, const NaturalIK::Parameters &params=NaturalIK::Parameters())
Definition: NaturalIK.cpp:219
armarx::NaturalIK::setScale
void setScale(float scale)
Definition: NaturalIK.cpp:189
armarx::NaturalIK::forwardKinematics
SoechtingForwardPositions forwardKinematics(SoechtingAngles sa)
Definition: NaturalIK.cpp:168
armarx::NaturalIK::Parameters::diffIKparams
NaturalDiffIK::Parameters diffIKparams
Definition: NaturalIK.h:55
armarx::NaturalIK::calculateIKpos
NaturalDiffIK::Result calculateIKpos(const Eigen::Vector3f &targetPos, ArmJoints arm, NaturalIK::Parameters params=NaturalIK::Parameters())
Definition: NaturalIK.cpp:91
armarx::NaturalIK::getLowerArmLength
float getLowerArmLength() const
Definition: NaturalIK.cpp:209
armarx::NaturalIK::ArmJoints::rns
VirtualRobot::RobotNodeSetPtr rns
Definition: NaturalIK.h:61
armarx::NaturalIK::SoechtingAngles::MMM_UPPER_ARM_LENGTH
static constexpr float MMM_UPPER_ARM_LENGTH
Definition: NaturalIK.h:79
armarx::NaturalIK::SoechtingAngles::MMM_CLAVICULAR_WIDTH
static constexpr float MMM_CLAVICULAR_WIDTH
Definition: NaturalIK.h:85
armarx::NaturalIK::SoechtingForwardPositions::elbow
Eigen::Vector3f elbow
Definition: NaturalIK.h:92
armarx::NaturalIK::getShoulderPos
Eigen::Vector3f getShoulderPos()
Definition: NaturalIK.cpp:106
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::NaturalIK::SoechtingAngles::SE
float SE
Definition: NaturalIK.h:70
armarx::NaturalIK::Parameters::minimumElbowHeight
std::optional< float > minimumElbowHeight
Definition: NaturalIK.h:56
armarx::NaturalIK::SoechtingAngles::MMM_BT2LSC
static constexpr float MMM_BT2LSC
Definition: NaturalIK.h:83
armarx::NaturalIK::SoechtingForwardPositions
Definition: NaturalIK.h:89
armarx::NaturalDiffIK::Parameters
Definition: NaturalDiffIK.h:37
armarx::NaturalDiffIK::Result
Definition: NaturalDiffIK.h:69
armarx::NaturalIKProvider::SolveAbsolute
DiffIKResult SolveAbsolute(const Eigen::Matrix4f &targetPose)
Definition: NaturalIK.cpp:224
armarx::NaturalIKProvider::SolveRelative
DiffIKResult SolveRelative(const Eigen::Matrix4f &targetPose, const Eigen::VectorXf &startJointValues)
Definition: NaturalIK.cpp:236
armarx::NaturalIK::solveSoechtingIK
NaturalIK::SoechtingForwardPositions solveSoechtingIK(const Eigen::Vector3f &targetPos, std::optional< float > minElbowHeight=std::nullopt)
Definition: NaturalIK.cpp:39
armarx::NaturalIK::SoechtingAngles::EY
float EY
Definition: NaturalIK.h:73
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::NaturalIK::SoechtingAngles::MMM_LS2LE
static constexpr float MMM_LS2LE
Definition: NaturalIK.h:76
armarx::NaturalIK::SoechtingForwardPositions::soechtingAngles
SoechtingAngles soechtingAngles
Definition: NaturalIK.h:94
armarx::NaturalIKProvider
Definition: NaturalIK.h:142
armarx::NaturalIK::getUpperArmLength
float getUpperArmLength() const
Definition: NaturalIK.cpp:199
armarx::DiffIKProvider
Definition: DiffIKProvider.h:43
armarx::NaturalIK::SoechtingForwardPositions::shoulder
Eigen::Vector3f shoulder
Definition: NaturalIK.h:91