reflex.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package
19  * @author
20  * @date
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 #pragma once
25 
27 
28 #include <Eigen/Core>
29 #include <Eigen/Geometry>
30 
31 #include <mutex>
32 
33 namespace armarx
34 {
35  class ReflexCombination;
36  class Reflex
37  {
38  public:
40  {
41  this->interval = interval;
42 
43  task = new PeriodicTask<Reflex>(this, &Reflex::calc, interval, false, "HeadStabilizationTask");
44  task->setDelayWarningTolerance(5);
45  }
46 
47  virtual ~Reflex()
48  {
49  stop();
50  }
51 
52 
53  void start()
54  {
55  if (!task->isRunning())
56  {
57  task->start();
58  }
59  }
60 
61  void stop()
62  {
63  if (task->isRunning())
64  {
65  task->stop();
66  }
67 
68  onStop();
69 
70  jointAngles.clear();
71  }
72 
73  void setEnabled(bool enabled)
74  {
75  if (enabled)
76  {
77  start();
78  }
79  else
80  {
81  stop();
82  }
83  }
84 
85  std::map<std::string, float> getJoints()
86  {
87  std::scoped_lock lock(mutex);
88 
89  std::map<std::string, float> result(jointAngles);
90 
91  //if (( currentTime - updateTime) > xxx) {
92 
93  //result.swap(jointAngles);
94  // }
95 
96  return result;
97  }
98 
99 
100  float getWeight() const
101  {
102  return weight;
103  }
104 
105  void setWeight(float weight)
106  {
107  this->weight = weight;
108  }
109 
110  virtual std::string getName() const = 0;
111 
112 
113  protected:
114 
116  {
117  Eigen::Vector3f euler;
118 
119  euler(0) = -std::atan2(2.0 * (q.w() * q.x() - q.y() * q.z()), 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y())); //roll
120  euler(1) = std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())); //pitch
121  euler(2) = std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); //yaw
122 
123  return euler;
124  }
125 
126  virtual void calc() = 0;
127 
128  virtual void onStop() = 0;
129 
130  std::mutex mutex;
131 
132  std::map<std::string, float> jointAngles;
133 
134  int interval;
135 
136  bool isEnabled;
137 
139 
140  std::string name;
141 
142 
143  private:
144 
146 
147  float weight;
148 
149  };
150 
151 
152  using ReflexPtr = std::shared_ptr<Reflex>;
153 }
armarx::Reflex::onStop
virtual void onStop()=0
armarx::Reflex::setWeight
void setWeight(float weight)
Definition: reflex.h:105
armarx::Reflex::updateTime
IceUtil::Time updateTime
Definition: reflex.h:138
PeriodicTask.h
armarx::Reflex::getWeight
float getWeight() const
Definition: reflex.h:100
armarx::Reflex::stop
void stop()
Definition: reflex.h:61
armarx::Reflex::jointAngles
std::map< std::string, float > jointAngles
Definition: reflex.h:132
armarx::Reflex::name
std::string name
Definition: reflex.h:140
armarx::Reflex::calc
virtual void calc()=0
armarx::Reflex::~Reflex
virtual ~Reflex()
Definition: reflex.h:47
armarx::Reflex
Definition: reflex.h:36
armarx::Reflex::mutex
std::mutex mutex
Definition: reflex.h:130
armarx::Reflex::getJoints
std::map< std::string, float > getJoints()
Definition: reflex.h:85
enabled
std::atomic< bool > * enabled
Definition: RemoteGuiWidgetController.cpp:75
armarx::Reflex::isEnabled
bool isEnabled
Definition: reflex.h:136
armarx::Reflex::getName
virtual std::string getName() const =0
q
#define q
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::Reflex::quaternionToRPY
Eigen::Vector3f quaternionToRPY(Eigen::Quaternionf q)
Definition: reflex.h:115
armarx::Quaternion< float, 0 >
IceUtil::Handle
Definition: forward_declarations.h:29
armarx::Reflex::Reflex
Reflex(int interval)
Definition: reflex.h:39
armarx::PeriodicTask
Definition: ArmarXManager.h:70
armarx::Reflex::interval
int interval
Definition: reflex.h:134
armarx::ReflexPtr
std::shared_ptr< Reflex > ReflexPtr
Definition: reflex.h:152
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::Reflex::start
void start()
Definition: reflex.h:53
armarx::Reflex::setEnabled
void setEnabled(bool enabled)
Definition: reflex.h:73