miniquadrotor_entity.cpp
Go to the documentation of this file.
1 
7 #include "miniquadrotor_entity.h"
8 
9 #include <argos3/core/simulator/space/space.h>
10 #include <argos3/core/simulator/entity/controllable_entity.h>
11 #include <argos3/core/simulator/entity/embodied_entity.h>
12 #include <argos3/plugins/simulator/entities/rotor_equipped_entity.h>
13 
14 namespace argos {
15 
16  /****************************************/
17  /****************************************/
18 
19  static const Real ARM_LENGTH = 0.63f;
20 
21  /****************************************/
22  /****************************************/
23 
24  CMiniQuadrotorEntity::CMiniQuadrotorEntity() :
25  CComposableEntity(NULL),
26  m_pcControllableEntity(NULL),
27  m_pcEmbodiedEntity(NULL),
28  m_pcRotorEquippedEntity(NULL) {
29  }
30 
31  /****************************************/
32  /****************************************/
33 
34  CMiniQuadrotorEntity::CMiniQuadrotorEntity(const std::string& str_id,
35  const std::string& str_controller_id,
36  const CVector3& c_position,
37  const CQuaternion& c_orientation) :
38  CComposableEntity(NULL, str_id),
39  m_pcControllableEntity(NULL),
40  m_pcEmbodiedEntity(NULL),
41  m_pcRotorEquippedEntity(NULL) {
42  try {
43  /*
44  * Create and init components
45  */
46  /*
47  * Embodied entity
48  * Better to put this first, because many other entities need this one
49  */
50  m_pcEmbodiedEntity = new CEmbodiedEntity(this, "body_0", c_position, c_orientation);
51  AddComponent(*m_pcEmbodiedEntity);
52  /* Rotor equipped entity */
53  m_pcRotorEquippedEntity = new CRotorEquippedEntity(this, "rotors_0", 4);
54  m_pcRotorEquippedEntity->SetRotor(0, CVector3(ARM_LENGTH, 0.0f, 0.0f));
55  m_pcRotorEquippedEntity->SetRotor(1, CVector3(0.0f, ARM_LENGTH, 0.0f));
56  m_pcRotorEquippedEntity->SetRotor(2, CVector3(-ARM_LENGTH, 0.0f, 0.0f));
57  m_pcRotorEquippedEntity->SetRotor(3, CVector3(0.0f, -ARM_LENGTH, 0.0f));
58  AddComponent(*m_pcRotorEquippedEntity);
59  /* Controllable entity
60  It must be the last one, for actuators/sensors to link to composing entities correctly */
61  m_pcControllableEntity = new CControllableEntity(this, "controller_0");
62  AddComponent(*m_pcControllableEntity);
63  m_pcControllableEntity->SetController(str_controller_id);
64  /* Update components */
65  UpdateComponents();
66  }
67  catch(CARGoSException& ex) {
68  THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
69  }
70  }
71 
72  /****************************************/
73  /****************************************/
74 
75  void CMiniQuadrotorEntity::Init(TConfigurationNode& t_tree) {
76  try {
77  /*
78  * Init parent
79  */
80  CComposableEntity::Init(t_tree);
81  /*
82  * Create and init components
83  */
84  /*
85  * Embodied entity
86  * Better to put this first, because many other entities need this one
87  */
88  m_pcEmbodiedEntity = new CEmbodiedEntity(this);
89  AddComponent(*m_pcEmbodiedEntity);
90  m_pcEmbodiedEntity->Init(GetNode(t_tree, "body"));
91  /* Rotor equipped entity */
92  m_pcRotorEquippedEntity = new CRotorEquippedEntity(this, "rotors_0", 4);
93  m_pcRotorEquippedEntity->SetRotor(0, CVector3(ARM_LENGTH, 0.0f, 0.0f));
94  m_pcRotorEquippedEntity->SetRotor(1, CVector3(0.0f, ARM_LENGTH, 0.0f));
95  m_pcRotorEquippedEntity->SetRotor(2, CVector3(-ARM_LENGTH, 0.0f, 0.0f));
96  m_pcRotorEquippedEntity->SetRotor(3, CVector3(0.0f, -ARM_LENGTH, 0.0f));
97  AddComponent(*m_pcRotorEquippedEntity);
98  /* Controllable entity
99  It must be the last one, for actuators/sensors to link to composing entities correctly */
100  m_pcControllableEntity = new CControllableEntity(this);
101  AddComponent(*m_pcControllableEntity);
102  m_pcControllableEntity->Init(GetNode(t_tree, "controller"));
103  /* Update components */
104  UpdateComponents();
105  }
106  catch(CARGoSException& ex) {
107  THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
108  }
109  }
110 
111  /****************************************/
112  /****************************************/
113 
114  REGISTER_ENTITY(CMiniQuadrotorEntity,
115  "mini-quadrotor",
116  "Carlo Pinciroli [ilpincy@gmail.com]",
117  "1.0",
118  "The mini-quadrotor robot, developed at UPenn.",
119  "The mini-quadrotor is a fluing robot developed at UPenn by Vijay Kumar's group.\n"
120  "It is a very agile robot, able to perform fast and precise movements.\n\n"
121  "REQUIRED XML CONFIGURATION\n\n"
122  " <arena ...>\n"
123  " ...\n"
124  " <mini-quadrotor id=\"mq0\">\n"
125  " <body position=\"0.4,2.3,2.0\" orientation=\"45,0,0\" />\n"
126  " <controller config=\"mycntrl\" />\n"
127  " </mini-quadrotor>\n"
128  " ...\n"
129  " </arena>\n\n"
130  "The 'id' attribute is necessary and must be unique among the entities. If two\n"
131  "entities share the same id, initialization aborts.\n"
132  "The 'body/position' attribute specifies the position of the bottom point of the\n"
133  "mini-quadrotor in the arena. When the robot is untranslated and unrotated, the\n"
134  "bottom point is in the origin and it is defined as the middle point between\n"
135  "the two wheels on the XY plane and the lowest point of the robot on the Z\n"
136  "axis, that is the point where the wheels touch the floor. The attribute values\n"
137  "are in the X,Y,Z order.\n"
138  "The 'body/orientation' attribute specifies the orientation of the\n"
139  "mini-quadrotor. All rotations are performed with respect to the bottom point.\n"
140  "The order of the angles is Z,Y,X, which means that the first number corresponds\n"
141  "to the rotation around the Z axis, the second around Y and the last around X.\n"
142  "This reflects the internal convention used in ARGoS, in which rotations are\n"
143  "performed in that order. Angles are expressed in degrees. When the robot is\n"
144  "unrotated, it is oriented along the X axis.\n"
145  "The 'controller/config' attribute is used to assign a controller to the\n"
146  "mini-quadrotor. The value of the attribute must be set to the id of a previously\n"
147  "defined controller. Controllers are defined in the <controllers> XML subtree.\n\n"
148  "OPTIONAL XML CONFIGURATION\n\n"
149  "None.\n\n"
150  ,
151  "Under development"
152  );
153 
154  /****************************************/
155  /****************************************/
156 
157  REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CMiniQuadrotorEntity);
158 
159  /****************************************/
160  /****************************************/
161 
162 }
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
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::GetNode
TConfigurationNode & GetNode(TConfigurationNode &t_node, const std::string &str_tag)
Given a tree root node, returns the first of its child nodes with the wanted name.
Definition: argos_configuration.h:63
argos::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition: argos_configuration.h:27
argos::REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE
REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CComposableEntity)
argos::REGISTER_ENTITY
REGISTER_ENTITY(CFloorEntity, "floor", "Carlo Pinciroli [ilpincy@gmail.com]", "1.0", "It contains the properties of the arena floor.", "The floor entity contains the properties of the arena floor. In the current\n" "implementation, it contains only the color of the floor. The floor color is\n" "detected by the robots' ground sensors.\n\n" "REQUIRED XML CONFIGURATION\n\n" " <arena ...>\n" " ...\n" " <floor id=\"floor\"\n" " source=\"SOURCE\" />\n" " ...\n" " </arena>\n\n" "The 'id' attribute is necessary and must be unique among the entities. If two\n" "entities share the same id, initialization aborts.\n" "The 'source' attribute specifies where to get the color of the floor from. Its\n" "value, here denoted as SOURCE, can assume the following values:\n\n" " image The color is calculated from the passed image file\n" " loop_functions The color is calculated calling the loop functions\n\n" "When 'source' is set to 'image', as showed in the following example, you have\n" "to specify the image path in the additional attribute 'path':\n\n" " <arena ...>\n" " ...\n" " <floor id=\"floor\"\n" " source=\"image\"\n" " path=\"/path/to/imagefile.ext\" />\n" " ...\n" " </arena>\n\n" "Many image formats are available, such as PNG, JPG, BMP, GIF and many more.\n" "Refer to the FreeImage webpage for a complete list of supported image formats\n" "(http://freeimage.sourceforge.net/features.html).\n\n" "When 'source' is set to 'loop_functions', as showed in the following example,\n" "an image is implicitly created to be used as texture for graphical\n" "visualizations. The algorithm that creates the texture needs to convert from\n" "meters (in the arena) to pixels (of the texture). You control how many pixels\n" "per meter are used with the attribute 'pixels_per_meter'. Clearly, the higher\n" "value, the higher the quality, but also the slower the algorithm and the bigger\n" "the texture. The algorithm is called only once at init time, so the fact that\n" "it is slow is not so important. However, the image size is limited by OpenGL.\n" "Every implementation has its own limit, and you should check yours if any\n" "texture-related problem arises. Now for the example:\n\n" " <arena ...>\n" " ...\n" " <floor id=\"floor\"\n" " source=\"loop_functions\"\n" " pixels_per_meter=\"100\" />\n" " ...\n" " </arena>\n\n" "OPTIONAL XML CONFIGURATION\n\n" "None for the time being.\n", "Usable")
argos::CComposableEntity
Basic class for an entity that contains other entities.
Definition: composable_entity.h:32
argos::CComposableEntity::UpdateComponents
virtual void UpdateComponents()
Calls the Update() method on all the components.
Definition: composable_entity.cpp:59
argos::CComposableEntity::AddComponent
void AddComponent(CEntity &c_component)
Adds a component to this composable entity.
Definition: composable_entity.cpp:72
argos::CControllableEntity
An entity that contains a pointer to the user-defined controller.
Definition: controllable_entity.h:26
argos::CControllableEntity::SetController
void SetController(const std::string &str_controller_id)
Creates and assigns a controller with the given id.
Definition: controllable_entity.cpp:139
argos::CControllableEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: controllable_entity.cpp:44
argos::CEmbodiedEntity
This entity is a link to a body in the physics engine.
Definition: embodied_entity.h:48
argos::CEmbodiedEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: embodied_entity.cpp:68
argos::CEntity::GetId
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
argos::CEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: entity.cpp:40
argos::CARGoSException
The exception that wraps all errors in ARGoS.
Definition: argos_exception.h:61
argos::CVector3
A 3D vector class.
Definition: vector3.h:31
argos::CMiniQuadrotorEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: miniquadrotor_entity.cpp:75
argos::CRotorEquippedEntity::SetRotor
void SetRotor(UInt32 un_index, const CVector3 &c_position)
Definition: rotor_equipped_entity.cpp:57