differential_steering_default_actuator.cpp
Go to the documentation of this file.
1 
8 #include <argos3/core/utility/logging/argos_log.h>
9 #include <argos3/core/utility/plugins/factory.h>
10 
11 namespace argos {
12 
13  /****************************************/
14  /****************************************/
15 
17  m_pcWheeledEntity(NULL),
18  m_pcRNG(NULL) {
21  m_fNoiseBias[LEFT_WHEEL] = 1.0;
27  }
28 
29  /****************************************/
30  /****************************************/
31 
33  try {
34  m_pcWheeledEntity = &(c_entity.GetComponent<CWheeledEntity>("wheels"));
35  if(m_pcWheeledEntity->GetNumWheels() != 2) {
36  THROW_ARGOSEXCEPTION("The differential steering actuator can be associated only to a robot with 2 wheels");
37  }
39  }
40  catch(CARGoSException& ex) {
41  THROW_ARGOSEXCEPTION_NESTED("Error setting differential steering actuator to entity \"" << c_entity.GetId() << "\"", ex);
42  }
43  }
44 
45  /****************************************/
46  /****************************************/
47 
48 #define CHECK_ATTRIBUTE(ATTR) \
49  (NodeAttributeExists(t_tree, ATTR) || \
50  NodeAttributeExists(t_tree, ATTR "_left") || \
51  NodeAttributeExists(t_tree, ATTR "_right"))
52 
53 #define PARSE_ATTRIBUTES(ATTR, VAR) \
54  GetNodeAttributeOrDefault<Real>(t_tree, ATTR, VAR[LEFT_WHEEL], VAR[LEFT_WHEEL]); \
55  VAR[RIGHT_WHEEL] = VAR[LEFT_WHEEL]; \
56  GetNodeAttributeOrDefault<Real>(t_tree, ATTR "_left", VAR[LEFT_WHEEL], VAR[LEFT_WHEEL]); \
57  GetNodeAttributeOrDefault<Real>(t_tree, ATTR "_right", VAR[RIGHT_WHEEL], VAR[RIGHT_WHEEL]);
58 
59 #define PICK_BIAS(LRW) m_fNoiseBias[LRW ## _WHEEL] = m_pcRNG->Gaussian(fNoiseBiasStdDev[LRW ## _WHEEL], fNoiseBiasAvg[LRW ## _WHEEL])
60 
62  try {
64  /* Check if any noise attribute was specified */
65  bool bNoise =
66  CHECK_ATTRIBUTE("bias_avg") ||
67  CHECK_ATTRIBUTE("bias_stddev") ||
68  CHECK_ATTRIBUTE("factor_avg") ||
69  CHECK_ATTRIBUTE("factor_stddev");
70  /* Handle noise attributes, if any */
71  if(bNoise) {
72  /* Create RNG */
73  m_pcRNG = CRandom::CreateRNG("argos");
74  /* Parse noise attributes */
75  Real fNoiseBiasAvg[2];
76  Real fNoiseBiasStdDev[2];
77  PARSE_ATTRIBUTES("bias_avg", fNoiseBiasAvg);
78  PARSE_ATTRIBUTES("bias_stddev", fNoiseBiasStdDev);
79  PARSE_ATTRIBUTES("factor_avg", m_fNoiseFactorAvg);
80  PARSE_ATTRIBUTES("factor_stddev", m_fNoiseFactorStdDev);
81  /* Choose robot bias */
82  PICK_BIAS(LEFT);
83  PICK_BIAS(RIGHT);
84  }
85  }
86  catch(CARGoSException& ex) {
87  THROW_ARGOSEXCEPTION_NESTED("Initialization error in foot-bot steering actuator.", ex);
88  }
89  }
90 
91  /****************************************/
92  /****************************************/
93 
94 #define ADD_GAUSSIAN(LRW) \
95  (m_fNoiseFactorStdDev[LRW ## _WHEEL] > 0.0 ? \
96  m_pcRNG->Gaussian(m_fNoiseFactorStdDev[LRW ## _WHEEL], \
97  m_fNoiseFactorAvg[LRW ## _WHEEL]) : \
98  m_fNoiseFactorAvg[LRW ## _WHEEL])
99 
100 #define ADD_NOISE(LRW) \
101  m_fCurrentVelocity[LRW ## _WHEEL] = \
102  ADD_GAUSSIAN(LRW) \
103  * \
104  (m_fCurrentVelocity[LRW ## _WHEEL] + \
105  m_fNoiseBias[LRW ## _WHEEL]);
106 
108  Real f_right_velocity) {
109  /* Convert speeds in m/s */
110  m_fCurrentVelocity[LEFT_WHEEL] = f_left_velocity * 0.01;
111  m_fCurrentVelocity[RIGHT_WHEEL] = f_right_velocity * 0.01;
112  /* Apply noise */
113  if(m_pcRNG) {
114  ADD_NOISE(LEFT);
115  ADD_NOISE(RIGHT);
116  }
117  }
118 
119  /****************************************/
120  /****************************************/
121 
124  }
125 
126  /****************************************/
127  /****************************************/
128 
130  /* Zero the speeds */
133  /*
134  * Throw away two RNG Gaussian calls to make sure the RNG sequence is the same after resetting
135  * These two calls were used to pick the bias in Init()
136  */
137  if(m_pcRNG) {
138  m_pcRNG->Gaussian(1.0, 0.0);
139  m_pcRNG->Gaussian(1.0, 0.0);
140  }
141  }
142 
143  /****************************************/
144  /****************************************/
145 
146 }
147 
148 REGISTER_ACTUATOR(CDifferentialSteeringDefaultActuator,
149  "differential_steering", "default",
150  "Carlo Pinciroli [ilpincy@gmail.com]",
151  "1.0",
152  "The differential steering actuator.",
153  "This actuator controls the two wheels a differential steering robot. For a\n"
154  "complete description of its usage, refer to the\n"
155  "ci_differential_steering_actuator.h file.\n\n"
156  "REQUIRED XML CONFIGURATION\n\n"
157  " <controllers>\n"
158  " ...\n"
159  " <my_controller ...>\n"
160  " ...\n"
161  " <actuators>\n"
162  " ...\n"
163  " <differential_steering implementation=\"default\" />\n"
164  " ...\n"
165  " </actuators>\n"
166  " ...\n"
167  " </my_controller>\n"
168  " ...\n"
169  " </controllers>\n\n"
170  "OPTIONAL XML CONFIGURATION\n\n"
171  "It is possible to specify noisy speed in order to match the characteristics\n"
172  "of the real robot. For each wheel, the noise model is as follows:\n\n"
173  "w = ideal wheel actuation (as set in the controller)\n"
174  "b = random bias from a Gaussian distribution\n"
175  "f = random factor from a Gaussian distribution\n"
176  "a = actual actuated value\n\n"
177  "a = f * (w + b)\n\n"
178  "You can configure the average and stddev of both the bias and the factor. This\n"
179  "can be done with the optional attributes: 'bias_avg', 'bias_stddev',\n"
180  "'factor_avg', and 'factor_stddev'. Bias attributes are expressed in m/s, while\n"
181  "factor attributes are dimensionless. If none of these attributed is specified,\n"
182  "no noise is added. If at least one of these attributed is specified, noise is\n"
183  "added and, for the non-specified attributes, the default value of 1 is used for\n"
184  "the '*_avg' attributes, while 0 is used for '*_stddev' attributes. Examples:\n\n"
185  " <controllers>\n"
186  " ...\n"
187  " <my_controller ...>\n"
188  " ...\n"
189  " <actuators>\n"
190  " ...\n"
191  " <!-- Only the stddev of the bias\n"
192  " Noise is on, other attributes are default -->\n"
193  " <differential_steering implementation=\"default\"\n"
194  " bias_stddev=\"2\" />\n"
195  " <!-- Only the stddev of the factor\n"
196  " Noise is on, other attributes are default -->\n"
197  " <differential_steering implementation=\"default\"\n"
198  " factor_stddev=\"4\" />\n"
199  " <!-- All attributes set\n"
200  " Noise is on, specified values are set -->\n"
201  " <differential_steering implementation=\"default\"\n"
202  " bias_avg=\"1\"\n"
203  " bias_stddev=\"2\"\n"
204  " factor_avg=\"3\"\n"
205  " factor_stddev=\"4\" />\n"
206  " ...\n"
207  " </actuators>\n"
208  " ...\n"
209  " </my_controller>\n"
210  " ...\n"
211  " </controllers>\n\n"
212  "The above examples set the same noise for both wheels. If you want to set\n"
213  "different noise parameters for each wheel, append '_left' and '_right' to the\n"
214  "attribute names:\n\n"
215  " <controllers>\n"
216  " ...\n"
217  " <my_controller ...>\n"
218  " ...\n"
219  " <actuators>\n"
220  " ...\n"
221  " <!-- Mix of wheel-specific attributes set\n"
222  " Noise is on, specified values are set -->\n"
223  " <differential_steering implementation=\"default\"\n"
224  " bias_avg_left=\"1\"\n"
225  " bias_stddev_right=\"2\"\n"
226  " factor_avg_left=\"3\"\n"
227  " factor_stddev_right=\"4\" />\n"
228  " ...\n"
229  " </actuators>\n"
230  " ...\n"
231  " </my_controller>\n"
232  " ...\n"
233  " </controllers>\n\n"
234  "Wheel-specific attributes overwrite the values of non-wheel specific attributes.\n"
235  "So, if you set 'bias_avg' = 2 and then 'bias_avg_left' = 3, the left wheel will\n"
236  "use 3 and the right wheel will use 2.\n\n"
237  "Physics-engine-specific attributes that affect this actuator might also be\n"
238  "available. Check the documentation of the physics engine you're using for more\n"
239  "information.",
240  "Usable"
241  );
242 
#define PARSE_ATTRIBUTES(ATTR, VAR)
void Enable()
Enables the entity.
Definition: entity.h:265
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
virtual void SetLinearVelocity(Real f_left_velocity, Real f_right_velocity)
Sets the linear velocity of the two steering.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Real m_fNoiseFactorAvg[2]
Noise factor average (Gaussian model) for each wheel.
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
virtual void Init(TConfigurationNode &t_tree)
Initializes the actuator from the XML configuration tree.
virtual void Reset()
Resets the actuator to the state it had just after Init().
virtual void Init(TConfigurationNode &t_node)
Initializes the actuator from the XML configuration tree.
Definition: ci_actuator.h:54
Basic class for an entity that contains other entities.
virtual void SetRobot(CComposableEntity &c_entity)
Sets the entity associated to this actuator.
The exception that wraps all errors in ARGoS.
Real m_fNoiseFactorStdDev[2]
Noise factor stddev (Gaussian model) for each wheel.
Real Gaussian(Real f_std_dev, Real f_mean=0.0f)
Returns a random value from a Gaussian distribution.
Definition: rng.cpp:131
REGISTER_ACTUATOR(CDifferentialSteeringDefaultActuator,"differential_steering","default","Carlo Pinciroli [ilpincy@gmail.com]","1.0","The differential steering actuator.","This actuator controls the two wheels a differential steering robot. For a\n""complete description of its usage, refer to the\n""ci_differential_steering_actuator.h file.\n\n""REQUIRED XML CONFIGURATION\n\n"" <controllers>\n"" ...\n"" <my_controller ...>\n"" ...\n"" <actuators>\n"" ...\n"" <differential_steering implementation=\"default\" />\n"" ...\n"" </actuators>\n"" ...\n"" </my_controller>\n"" ...\n"" </controllers>\n\n""OPTIONAL XML CONFIGURATION\n\n""It is possible to specify noisy speed in order to match the characteristics\n""of the real robot. For each wheel, the noise model is as follows:\n\n""w = ideal wheel actuation (as set in the controller)\n""b = random bias from a Gaussian distribution\n""f = random factor from a Gaussian distribution\n""a = actual actuated value\n\n""a = f * (w + b)\n\n""You can configure the average and stddev of both the bias and the factor. This\n""can be done with the optional attributes: 'bias_avg', 'bias_stddev',\n""'factor_avg', and 'factor_stddev'. Bias attributes are expressed in m/s, while\n""factor attributes are dimensionless. If none of these attributed is specified,\n""no noise is added. If at least one of these attributed is specified, noise is\n""added and, for the non-specified attributes, the default value of 1 is used for\n""the '*_avg' attributes, while 0 is used for '*_stddev' attributes. Examples:\n\n"" <controllers>\n"" ...\n"" <my_controller ...>\n"" ...\n"" <actuators>\n"" ...\n"" <!-- Only the stddev of the bias\n"" Noise is on, other attributes are default -->\n"" <differential_steering implementation=\"default\"\n"" bias_stddev=\"2\" />\n"" <!-- Only the stddev of the factor\n"" Noise is on, other attributes are default -->\n"" <differential_steering implementation=\"default\"\n"" factor_stddev=\"4\" />\n"" <!-- All attributes set\n"" Noise is on, specified values are set -->\n"" <differential_steering implementation=\"default\"\n"" bias_avg=\"1\"\n"" bias_stddev=\"2\"\n"" factor_avg=\"3\"\n"" factor_stddev=\"4\" />\n"" ...\n"" </actuators>\n"" ...\n"" </my_controller>\n"" ...\n"" </controllers>\n\n""The above examples set the same noise for both wheels. If you want to set\n""different noise parameters for each wheel, append '_left' and '_right' to the\n""attribute names:\n\n"" <controllers>\n"" ...\n"" <my_controller ...>\n"" ...\n"" <actuators>\n"" ...\n"" <!-- Mix of wheel-specific attributes set\n"" Noise is on, specified values are set -->\n"" <differential_steering implementation=\"default\"\n"" bias_avg_left=\"1\"\n"" bias_stddev_right=\"2\"\n"" factor_avg_left=\"3\"\n"" factor_stddev_right=\"4\" />\n"" ...\n"" </actuators>\n"" ...\n"" </my_controller>\n"" ...\n"" </controllers>\n\n""Wheel-specific attributes overwrite the values of non-wheel specific attributes.\n""So, if you set 'bias_avg' = 2 and then 'bias_avg_left' = 3, the left wheel will\n""use 3 and the right wheel will use 2.\n\n""Physics-engine-specific attributes that affect this actuator might also be\n""available. Check the documentation of the physics engine you're using for more\n""information.","Usable")
static CRNG * CreateRNG(const std::string &str_category)
Creates a new RNG inside the given category.
Definition: rng.cpp:326
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
void SetVelocities(Real *pf_velocities)
size_t GetNumWheels() const
#define CHECK_ATTRIBUTE(ATTR)
virtual void Update()
Updates the state of the entity associated to this actuator.
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.