differential_steering_default_actuator.cpp
Go to the documentation of this file.
1 
7 #include "differential_steering_default_actuator.h"
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 
16  CDifferentialSteeringDefaultActuator::CDifferentialSteeringDefaultActuator() :
17  m_pcWheeledEntity(nullptr),
18  m_pcRNG(nullptr) {
19  m_fCurrentVelocity[LEFT_WHEEL] = 0.0;
20  m_fCurrentVelocity[RIGHT_WHEEL] = 0.0;
21  m_fNoiseBias[LEFT_WHEEL] = 1.0;
22  m_fNoiseBias[RIGHT_WHEEL] = 1.0;
23  m_fNoiseFactorAvg[LEFT_WHEEL] = 1.0;
24  m_fNoiseFactorAvg[RIGHT_WHEEL] = 1.0;
25  m_fNoiseFactorStdDev[LEFT_WHEEL] = 0.0;
26  m_fNoiseFactorStdDev[RIGHT_WHEEL] = 0.0;
27  }
28 
29  /****************************************/
30  /****************************************/
31 
32  void CDifferentialSteeringDefaultActuator::SetRobot(CComposableEntity& c_entity) {
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  }
38  m_pcWheeledEntity->Enable();
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 
61  void CDifferentialSteeringDefaultActuator::Init(TConfigurationNode& t_tree) {
62  try {
63  CCI_DifferentialSteeringActuator::Init(t_tree);
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 
107  void CDifferentialSteeringDefaultActuator::SetLinearVelocity(Real f_left_velocity,
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 only if the robot is in motion (at least one of the wheels is moving)*/
113  if( m_pcRNG &&
114  ((f_left_velocity != 0) ||
115  (f_right_velocity != 0) )) {
116  ADD_NOISE(LEFT);
117  ADD_NOISE(RIGHT);
118  }
119  }
120 
121  /****************************************/
122  /****************************************/
123 
124  void CDifferentialSteeringDefaultActuator::Update() {
125  m_pcWheeledEntity->SetVelocities(m_fCurrentVelocity);
126  }
127 
128  /****************************************/
129  /****************************************/
130 
131  void CDifferentialSteeringDefaultActuator::Reset() {
132  /* Zero the speeds */
133  m_fCurrentVelocity[LEFT_WHEEL] = 0.0;
134  m_fCurrentVelocity[RIGHT_WHEEL] = 0.0;
135  /*
136  * Throw away two RNG Gaussian calls to make sure the RNG sequence is the same after resetting
137  * These two calls were used to pick the bias in Init()
138  */
139  if(m_pcRNG) {
140  m_pcRNG->Gaussian(1.0, 0.0);
141  m_pcRNG->Gaussian(1.0, 0.0);
142  }
143  }
144 
145  /****************************************/
146  /****************************************/
147 
148 }
149 
150 REGISTER_ACTUATOR(CDifferentialSteeringDefaultActuator,
151  "differential_steering", "default",
152  "Carlo Pinciroli [ilpincy@gmail.com]",
153  "1.0",
154  "The differential steering actuator.",
155 
156  "This actuator controls the two wheels a differential steering robot. For a\n"
157  "complete description of its usage, refer to the\n"
158  "ci_differential_steering_actuator.h file.\n\n"
159 
160  "REQUIRED XML CONFIGURATION\n\n"
161  " <controllers>\n"
162  " ...\n"
163  " <my_controller ...>\n"
164  " ...\n"
165  " <actuators>\n"
166  " ...\n"
167  " <differential_steering implementation=\"default\" />\n"
168  " ...\n"
169  " </actuators>\n"
170  " ...\n"
171  " </my_controller>\n"
172  " ...\n"
173  " </controllers>\n\n"
174 
175  "OPTIONAL XML CONFIGURATION\n\n"
176 
177  "It is possible to specify noisy speed in order to match the characteristics\n"
178  "of the real robot. For each wheel, the noise model is as follows:\n\n"
179  "w = ideal wheel actuation (as set in the controller)\n"
180  "b = random bias from a Gaussian distribution\n"
181  "f = random factor from a Gaussian distribution\n"
182  "a = actual actuated value\n\n"
183  "a = f * (w + b)\n\n"
184  "You can configure the average and stddev of both the bias and the factor. This\n"
185  "can be done with the optional attributes: 'bias_avg', 'bias_stddev',\n"
186  "'factor_avg', and 'factor_stddev'. Bias attributes are expressed in m/s, while\n"
187  "factor attributes are dimensionless. If none of these attributed is specified,\n"
188  "no noise is added. If at least one of these attributed is specified, noise is\n"
189  "added and, for the non-specified attributes, the default value of 1 is used for\n"
190  "the '*_avg' attributes, while 0 is used for '*_stddev' attributes. Examples:\n\n"
191 
192  " <controllers>\n"
193  " ...\n"
194  " <my_controller ...>\n"
195  " ...\n"
196  " <actuators>\n"
197  " ...\n"
198  " <!-- Only the stddev of the bias\n"
199  " Noise is on, other attributes are default -->\n"
200  " <differential_steering implementation=\"default\"\n"
201  " bias_stddev=\"2\" />\n"
202  " <!-- Only the stddev of the factor\n"
203  " Noise is on, other attributes are default -->\n"
204  " <differential_steering implementation=\"default\"\n"
205  " factor_stddev=\"4\" />\n"
206  " <!-- All attributes set\n"
207  " Noise is on, specified values are set -->\n"
208  " <differential_steering implementation=\"default\"\n"
209  " bias_avg=\"1\"\n"
210  " bias_stddev=\"2\"\n"
211  " factor_avg=\"3\"\n"
212  " factor_stddev=\"4\" />\n"
213  " ...\n"
214  " </actuators>\n"
215  " ...\n"
216  " </my_controller>\n"
217  " ...\n"
218  " </controllers>\n\n"
219 
220  "The above examples set the same noise for both wheels. If you want to set\n"
221  "different noise parameters for each wheel, append '_left' and '_right' to the\n"
222  "attribute names:\n\n"
223 
224  " <controllers>\n"
225  " ...\n"
226  " <my_controller ...>\n"
227  " ...\n"
228  " <actuators>\n"
229  " ...\n"
230  " <!-- Mix of wheel-specific attributes set\n"
231  " Noise is on, specified values are set -->\n"
232  " <differential_steering implementation=\"default\"\n"
233  " bias_avg_left=\"1\"\n"
234  " bias_stddev_right=\"2\"\n"
235  " factor_avg_left=\"3\"\n"
236  " factor_stddev_right=\"4\" />\n"
237  " ...\n"
238  " </actuators>\n"
239  " ...\n"
240  " </my_controller>\n"
241  " ...\n"
242  " </controllers>\n\n"
243 
244  "Wheel-specific attributes overwrite the values of non-wheel specific attributes.\n"
245  "So, if you set 'bias_avg' = 2 and then 'bias_avg_left' = 3, the left wheel will\n"
246  "use 3 and the right wheel will use 2.\n\n"
247  "Physics-engine-specific attributes that affect this actuator might also be\n"
248  "available. Check the documentation of the physics engine you're using for more\n"
249  "information.",
250  "Usable"
251  );
THROW_ARGOSEXCEPTION_NESTED
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception.
Definition: argos_exception.h:115
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition: argos_exception.h:111
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
CHECK_ATTRIBUTE
#define CHECK_ATTRIBUTE(ATTR)
Definition: differential_steering_default_actuator.cpp:48
PARSE_ATTRIBUTES
#define PARSE_ATTRIBUTES(ATTR, VAR)
Definition: differential_steering_default_actuator.cpp:53
REGISTER_ACTUATOR
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")
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition: argos_configuration.h:27
argos::CCI_Actuator::Init
virtual void Init(TConfigurationNode &t_node)
Initializes the actuator from the XML configuration tree.
Definition: ci_actuator.h:54
argos::CComposableEntity
Basic class for an entity that contains other entities.
Definition: composable_entity.h:32
argos::CComposableEntity::GetComponent
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.
Definition: composable_entity.cpp:109
argos::CEntity::GetId
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
argos::CEntity::Enable
void Enable()
Enables the entity.
Definition: entity.h:265
argos::CARGoSException
The exception that wraps all errors in ARGoS.
Definition: argos_exception.h:61
argos::CRandom::CreateRNG
static CRNG * CreateRNG(const std::string &str_category)
Creates a new RNG inside the given category.
Definition: rng.cpp:347
argos::CRandom::CRNG::Gaussian
Real Gaussian(Real f_std_dev, Real f_mean=0.0f)
Returns a random value from a Gaussian distribution.
Definition: rng.cpp:152
argos::CDifferentialSteeringDefaultActuator::SetRobot
virtual void SetRobot(CComposableEntity &c_entity)
Sets the entity associated to this actuator.
Definition: differential_steering_default_actuator.cpp:32
argos::CDifferentialSteeringDefaultActuator::Reset
virtual void Reset()
Resets the actuator to the state it had just after Init().
Definition: differential_steering_default_actuator.cpp:131
argos::CDifferentialSteeringDefaultActuator::SetLinearVelocity
virtual void SetLinearVelocity(Real f_left_velocity, Real f_right_velocity)
Sets the linear velocity of the two steering.
Definition: differential_steering_default_actuator.cpp:107
argos::CDifferentialSteeringDefaultActuator::Update
virtual void Update()
Updates the state of the entity associated to this actuator.
Definition: differential_steering_default_actuator.cpp:124
argos::CDifferentialSteeringDefaultActuator::m_fNoiseFactorAvg
Real m_fNoiseFactorAvg[2]
Noise factor average (Gaussian model) for each wheel
Definition: differential_steering_default_actuator.h:76
argos::CDifferentialSteeringDefaultActuator::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the actuator from the XML configuration tree.
Definition: differential_steering_default_actuator.cpp:61
argos::CDifferentialSteeringDefaultActuator::m_fNoiseFactorStdDev
Real m_fNoiseFactorStdDev[2]
Noise factor stddev (Gaussian model) for each wheel
Definition: differential_steering_default_actuator.h:79
argos::CWheeledEntity::SetVelocities
void SetVelocities(Real *pf_velocities)
Definition: wheeled_entity.cpp:113
argos::CWheeledEntity::GetNumWheels
size_t GetNumWheels() const
Definition: wheeled_entity.h:34