ReflexCombination.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package RobotComponents::ArmarXObjects::ReflexCombination
17  * @author [Author Name] ( [Author Email] )
18  * @date 2016
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #pragma once
24 
25 
26 #include "reflex.h"
27 #include "vor.h"
28 #include "jointik.h"
29 #include "feedforward.h"
30 #include "okr.h"
31 
34 
35 #include <VirtualRobot/VirtualRobot.h>
36 #include <VirtualRobot/Robot.h>
37 #include <VirtualRobot/Nodes/RobotNode.h>
38 #include <VirtualRobot/RobotNodeSet.h>
39 #include <VirtualRobot/IK/GazeIK.h>
40 
43 
44 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
45 
46 #include <VisionX/interface/components/ImageSourceSelectionInterface.h>
47 
48 #include <RobotComponents/interface/GazeStabilizationInterface.h>
52 
53 namespace armarx
54 {
56  {
58  };
59 
66  {
67  public:
70  {
71  defineOptionalProperty<bool>("VelocityBasedControl", true, "Use velocity based reflexes");
72 
73  defineOptionalProperty<float>("kp", 1.0, "p part of regulator");
74  defineOptionalProperty<float>("ki", 0.0, "i part of regulator");
75  defineOptionalProperty<float>("kd", 0.0, "d part of regulator");
76 
77  defineOptionalProperty<float>("VOR", 0, "Vestibulo-ocular reflex weight");
78  defineOptionalProperty<float>("OKR", 1, "Opto-kinetic reflex weight");
79  defineOptionalProperty<float>("JointIK", 0, "Joint IK weight");
80  defineOptionalProperty<float>("AdvancedVOR", 0, "Advanced Vestibulo-ocular reflex weight");
81 
82  defineOptionalProperty<std::string>("KinematicUnitName", "Armar3KinematicUnit", "Name of the KinematicUnit of the Robot");
83  defineOptionalProperty<std::string>("IMUObserverName", "InertialMeasurementUnitObserver", "Name of the InertialMeasurementUnitObserver of the Robot");
84  defineOptionalProperty<std::string>("RobotStateName", "RobotStateComponent", "Name of the RobotState of the Robot");
85  defineOptionalProperty<std::string>("HeadIKName", "IKVirtualGazeNoEyes", "Name of the HeadIK");
86  defineOptionalProperty<std::string>("RobotNodeSetName", "Robot", "Name of the RobotNodeSet");
87 
88  defineOptionalProperty<std::string>("ImageSourceSelectionName", "ImageSourceSelection", "Name of the ImageSourceSelection component");
89  defineOptionalProperty<Ice::StringSeq>("ImageProviders", {"Armar3ImageProvider", "Armar3FovealImageProvider"}, "Name of the image provider to choose from. If image quality is high, the second provider will be used.");
90 
91  defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
92 
93  defineOptionalProperty<std::string>("CalibrationFile", "ArmarXSimulation/camera_simulator_640x480.txt", "Camera calibration file, will be made available in the SLICE interface");
94 
95  defineOptionalProperty<bool>("NeckPerturbation", false, "Add sinusoidal motion in the neck joint to test the reflexes");
96  defineOptionalProperty<bool>("reafferenceCombination", false, "Combined reflexes with IK based on the reafference principle (sensory cancelation)");
97  }
98  };
99 
100  class Reflex;
101  class VOR;
102  class JointIK;
103  class FeedforwardReflex;
104  class OKR;
105  class AdvancedVOR;
106 
119  virtual public armarx::Component,
120  virtual public armarx::GazeStabilizationInterface
121  {
122  public:
126  std::string getDefaultName() const override
127  {
128  return "ReflexCombination";
129  }
130 
131  void updateWeights(float vor, float okr, float jointIK, const Ice::Current& c = Ice::emptyCurrent) override;
132 
133  void setReafferenceMethod(bool isReafference, const Ice::Current& c = Ice::emptyCurrent) override;
134 
135 
136  protected:
140  void onInitComponent() override;
141 
145  void onConnectComponent() override;
146 
150  void onDisconnectComponent() override;
151 
155  void onExitComponent() override;
156 
161 
162 
163  private:
164 
165  void setImageQuality(float quality);
166 
167  std::mutex mutex;
168  std::map<std::string, NameValueMap> reflexValues;
169  std::map<std::string, ReflexPtr> reflexes;
170  std::vector<std::string> headJointNames;
171 
172  VOR* vor;
173  FeedforwardReflex* jointIK;
174  OKR* okr;
175 
176 
177  bool armar4, velocityBased, neckPerturbation;
178  bool vorEnabled, jointIKEnabled, okrEnabled;
179  std::string eye_pitch_left, eye_pitch_right, eye_yaw_left, eye_yaw_right, neck_roll;
180  float vorWeight, okrWeight, jointIKWeight;
181  float kp, ki, kd;
182 
183  COMBINATION_METHOD combinationMethod;
184 
185  PeriodicTask<ReflexCombination>::pointer_type execCombineReflexesTask;
186 
187 
188  //required for JointIK
189  VirtualRobot::RobotPtr robot;
190  VirtualRobot::RobotNodeSetPtr robotNodeSet;
191 
192  std::string headIKName;
193  RobotStateComponentInterfacePrx robotStateComponent;
194  Eigen::Quaternionf initial;
195 
196  NameValueMap targetJointAngles;
197 
198  KinematicUnitInterfacePrx kinUnitPrx;
199  DebugObserverInterfacePrx debugObserver;
201  ImageSourceSelectionInterfacePrx imageSourceSelection;
202  bool newHeadTarget;
203  //RobotStateComponentInterfacePrx robotStateComponent;
204 
205  std::map<std::string, IceInternal::Handle<armarx::filters::DerivationFilter>> VelocityFilters;
206  std::map<std::string, IceInternal::Handle<armarx::filters::MedianFilter>> IMU_GyroFilters;
207  std::map<std::string, IceInternal::Handle<armarx::filters::ButterworthFilter>> PreVelocityFilters;
208  std::map<std::string, IceInternal::Handle<armarx::filters::ButterworthFilter>> FlowFilters;
209 
210  double t_init;
211 
212  // PlatformUnitListener interface
213  public:
214  void reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
215  void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&) override;
216 
217  // KinematicUnitListener interface
218  public:
219  void reportControlModeChanged(const NameControlModeMap&, Ice::Long timestamp, bool, const Ice::Current& c = Ice::emptyCurrent) override;
220  void reportJointAngles(const NameValueMap& values, Ice::Long timestamp, bool valueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
221  void reportJointVelocities(const NameValueMap& values, Ice::Long timestamp, bool valueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
222  void reportJointTorques(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current& c = Ice::emptyCurrent) override;
223  void reportJointAccelerations(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current& c = Ice::emptyCurrent) override;
224  void reportJointCurrents(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current& c = Ice::emptyCurrent) override;
225  void reportJointMotorTemperatures(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current& c = Ice::emptyCurrent) override;
226  void reportJointStatuses(const NameStatusMap&, Ice::Long timestamp, bool, const Ice::Current& c = Ice::emptyCurrent) override;
227 
228 
229  private:
230  void combineReflexes();
231  Eigen::Vector3f quaternionToRPY(Eigen::Quaternionf q);
232 
233  // OpticalFlowListener interface
234  public:
235  void reportNewOpticalFlow(Ice::Float, Ice::Float, Ice::Float, Ice::Long, const Ice::Current&) override;
236 
237  // HeadIKUnitListener interface
238  public:
239  void reportHeadTargetChanged(const NameValueMap& targetJointAngles, const FramedPositionBasePtr& targetPosition, const Ice::Current& c = Ice::emptyCurrent) override;
240 
241  // InertialMeasurementUnitListener interface
242  public:
243  void reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c = Ice::emptyCurrent) override;
244 
245  // HeadIKUnitListener interface
246  public:
247  void reportHeadTargetChanged(const Ice::Current&)
248  {
249  }
250 
251  // TrackingErrorListener interface
252  public:
253  void reportNewTrackingError(Ice::Float pixelX, Ice::Float pixelY, Ice::Float angleX, Ice::Float angleY, const Ice::Current&) override;
254 
255 
256  };
257 }
DerivationFilter.h
armarx::ReflexCombination::reportJointTorques
void reportJointTorques(const NameValueMap &, Ice::Long timestamp, bool, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:595
armarx::COMBINATION_METHOD
COMBINATION_METHOD
Definition: ReflexCombination.h:55
armarx::ReflexCombination::reportHeadTargetChanged
void reportHeadTargetChanged(const NameValueMap &targetJointAngles, const FramedPositionBasePtr &targetPosition, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:651
RemoteRobot.h
armarx::ReflexCombination::onInitComponent
void onInitComponent() override
Definition: ReflexCombination.cpp:31
armarx::ReflexCombination::reportHeadTargetChanged
void reportHeadTargetChanged(const Ice::Current &)
Definition: ReflexCombination.h:247
DebugObserver.h
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:918
armarx::ReflexCombination::updateWeights
void updateWeights(float vor, float okr, float jointIK, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:220
armarx::ReflexCombination::reportJointMotorTemperatures
void reportJointMotorTemperatures(const NameValueMap &, Ice::Long timestamp, bool, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:607
armarx::WeightedSum
@ WeightedSum
Definition: ReflexCombination.h:57
okr.h
armarx::OKR
Definition: okr.h:33
Pose.h
armarx::ReflexCombination::onExitComponent
void onExitComponent() override
Definition: ReflexCombination.cpp:206
armarx::ReflexCombination::getDefaultName
std::string getDefaultName() const override
Definition: ReflexCombination.h:126
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
armarx::DebugObserverInterfacePrx
::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugObserverInterface > DebugObserverInterfacePrx
Definition: JointController.h:42
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
vor.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
ButterworthFilter.h
armarx::ReflexCombination::reportJointAngles
void reportJointAngles(const NameValueMap &values, Ice::Long timestamp, bool valueChanged, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:494
armarx::ReflexCombination::reportSensorValues
void reportSensorValues(const std::string &device, const std::string &name, const IMUData &values, const TimestampBasePtr &timestamp, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:679
feedforward.h
armarx::ReflexCombination::reportPlatformVelocity
void reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:482
armarx::ReflexCombination::reportControlModeChanged
void reportControlModeChanged(const NameControlModeMap &, Ice::Long timestamp, bool, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:490
armarx::VOR
Definition: vor.h:36
armarx::FeedforwardReflex
Definition: feedforward.h:46
armarx::ReflexCombination::reportJointAccelerations
void reportJointAccelerations(const NameValueMap &, Ice::Long timestamp, bool, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:599
armarx::VariantType::Long
const VariantTypeId Long
Definition: Variant.h:917
armarx::ReflexCombination::reportNewTrackingError
void reportNewTrackingError(Ice::Float pixelX, Ice::Float pixelY, Ice::Float angleX, Ice::Float angleY, const Ice::Current &) override
Definition: ReflexCombination.cpp:741
timestamp
std::string timestamp()
Definition: CartographerAdapter.cpp:85
armarx::ReflexCombination::reportJointCurrents
void reportJointCurrents(const NameValueMap &, Ice::Long timestamp, bool, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:603
armarx::ReflexCombination::reportPlatformOdometryPose
void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override
Definition: ReflexCombination.cpp:747
Eigen::Quaternionf
Quaternion< float, 0 > Quaternionf
Definition: EigenForwardDeclarations.h:61
armarx::ReflexCombinationPropertyDefinitions
Definition: ReflexCombination.h:64
q
#define q
Component.h
armarx::Reafference
@ Reafference
Definition: ReflexCombination.h:57
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:95
armarx::ReflexCombination::onConnectComponent
void onConnectComponent() override
Definition: ReflexCombination.cpp:167
armarx::ReflexCombination::reportJointStatuses
void reportJointStatuses(const NameStatusMap &, Ice::Long timestamp, bool, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:612
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
reflex.h
IceUtil::Handle< class PropertyDefinitionContainer >
MedianFilter.h
armarx::ReflexCombination::setReafferenceMethod
void setReafferenceMethod(bool isReafference, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:254
jointik.h
armarx::ReflexCombination::reportJointVelocities
void reportJointVelocities(const NameValueMap &values, Ice::Long timestamp, bool valueChanged, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ReflexCombination.cpp:559
armarx::ReflexCombinationPropertyDefinitions::ReflexCombinationPropertyDefinitions
ReflexCombinationPropertyDefinitions(std::string prefix)
Definition: ReflexCombination.h:68
armarx::ReflexCombination::reportNewOpticalFlow
void reportNewOpticalFlow(Ice::Float, Ice::Float, Ice::Float, Ice::Long, const Ice::Current &) override
Definition: ReflexCombination.cpp:616
armarx::ReflexCombination::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: ReflexCombination.cpp:214
armarx::DebugDrawerInterfacePrx
::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugDrawerInterface > DebugDrawerInterfacePrx
Definition: JointController.h:40
armarx::ReflexCombination
Brief description of class ReflexCombination.
Definition: ReflexCombination.h:118
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::ReflexCombination::onDisconnectComponent
void onDisconnectComponent() override
Definition: ReflexCombination.cpp:200