optimizer_goal.h
Go to the documentation of this file.
1 
18 #ifndef __INC_OPTIMIZER_GOAL
19 #define __INC_OPTIMIZER_GOAL
20 
21 #include <vector>
22 #include <frame.h>
23 #include <vecmath.h>
24 #include <log.h>
25 #include <utils.h>
26 #include <datapairs.h>
27 
28 namespace robotLibPbD {
29 
31 {
32 protected:
34  std::string function;
35 public:
38  void loadFromXml(CFrameContainer &frames, TiXmlElement* frameNode)
39  {
40  first = frames.getFrame(CConfiguration::getAttributeString(frameNode, "first", ""));
41  second = frames.getFrame(CConfiguration::getAttributeString(frameNode, "second", ""));
42  if (second == NULL)
43  second = first;
44 
45  function = CConfiguration::getAttributeString(frameNode, "value", "");
46  };
47 
48  virtual double getDistance()
49  {
50  double angle;
51  CVec axis;
52  CMatrix pose;
53  LOG_MSG(10, "First:\n%s", first->getRelativeToBase().toString().c_str());
54  LOG_MSG(10, "Second:\n%s", second->getRelativeToBase().toString().c_str());
55 
56 
57  pose = first->getRelativeToBase();
58  pose.invert();
59  pose.mul(pose, second->getRelativeToBase());
60 
61  //LOG_MSG(5, "Difference:\n%s", pose.toString().c_str());
62 
63  CMathLib::getRotationFromMatrix(pose, axis, angle);
64  axis *= angle;
65 
66  information.add("x", printToString("%f", pose.a[12]), true);
67  information.add("y", printToString("%f", pose.a[13]), true);
68  information.add("z", printToString("%f", pose.a[14]), true);
69  information.add("rx", printToString("%f", axis.x), true);
70  information.add("ry", printToString("%f", axis.y), true);
71  information.add("rz", printToString("%f", axis.z), true);
72 
73  return atof(information.resolveString(function).c_str());
74  };
75 
76  void getDistance(CVec &from, CVec &to)
77  {
78  from = first->getRelativeToBase()[3];
79  to = second->getRelativeToBase()[3];
80  };
81 
82  void getDistance(CVec &to)
83  {
84  CMatrix pose = first->getRelativeToBase();
85  pose.invert();
86  to = pose * second->getRelativeToBase()[3];
87  };
88 };
89 };
90 
91 #endif
PRECISION z
Definition: vecmath.h:60
std::string printToString(const char *format,...)
Definition: utils.cpp:149
void loadFromXml(CFrameContainer &frames, TiXmlElement *frameNode)
static const char * getAttributeString(TiXmlElement *node, const char *str, const char *def="")
virtual CMatrix getRelativeToBase()
Returns pose in the base (frame with no predecessor) frame.
Definition: frame.h:508
static void getRotationFromMatrix(const CMatrix &mat, CVec &axis, double &angle)
Transforms homogenous matrix into axis-angle representation.
Definition: vecmath.cpp:463
void resolveString(std::string input, std::string &output)
Definition: datapairs.cpp:284
Data Storage Class (Attribute-Value Pairs)
Definition: datapairs.h:33
Homogenous vector.
Definition: vecmath.h:57
Homogenous matrix.
Definition: vecmath.h:187
virtual double getDistance()
std::string toString(bool round=false)
Definition: vecmath.cpp:221
#define LOG_MSG(index, format,...)
Definition: log.h:32
Robot types with implemented inverse kinematics.
Definition: frame.h:39
void getDistance(CVec &from, CVec &to)
CFrame * getFrame(unsigned int id)
Definition: frame.h:224
PRECISION y
Definition: vecmath.h:60
void invert()
Inverts the matrix (only for homogenous matrices)
Definition: vecmath.h:461
PRECISION x
Definition: vecmath.h:60
void add(std::string attribute, std::string value, bool replace=false)
Adds the attribute-value pair attribute, value.
Definition: datapairs.cpp:160


asr_kinematic_chain_optimizer
Author(s): Aumann Florian, Heller Florian, Jäkel Rainer, Wittenbeck Valerij
autogenerated on Mon Jun 10 2019 12:35:36