prototype_joint_entity.cpp
Go to the documentation of this file.
1 
8 #include <argos3/plugins/robots/prototype/simulator/prototype_link_equipped_entity.h>
9 
10 namespace argos {
11 
12  /****************************************/
13  /****************************************/
14 
16  CComposableEntity(pc_parent),
17  m_pcParentLink(nullptr),
18  m_cParentLinkJointPosition(CVector3::ZERO),
19  m_cParentLinkJointOrientation(CRadians::ZERO, CVector3::Z),
20  m_pcChildLink(nullptr),
21  m_cChildLinkJointPosition(CVector3::ZERO),
22  m_cChildLinkJointOrientation(CRadians::ZERO, CVector3::Z),
23  m_bDisableCollision(true),
24  m_eType(EType::FIXED),
25  m_cJointAxis(CVector3::Z),
26  m_bHasLimit(false) {}
27 
28  /****************************************/
29  /****************************************/
30 
32  try {
33  /* Init parent */
35  /* Get parent and child links */
36  CPrototypeLinkEquippedEntity& cLinkEquippedEntity =
38  /* Get the disable collisions with parent flag */
40  "disable_collision",
41  m_bDisableCollision,
42  m_bDisableCollision);
43  /* Get joint type */
44  std::string strJointType;
45  GetNodeAttribute(t_tree, "type", strJointType);
46  if(strJointType == "fixed") {
47  m_eType = EType::FIXED;
48  }
49  else if(strJointType == "prismatic") {
50  m_eType = EType::PRISMATIC;
51  GetNodeAttribute(t_tree, "axis", m_cJointAxis);
52  if(NodeAttributeExists(t_tree, "limit")) {
53  m_bHasLimit = true;
54  GetNodeAttribute(t_tree, "limit", m_uLimit.Prismatic);
55  }
56  }
57  else if(strJointType == "revolute") {
58  m_eType = EType::REVOLUTE;
59  GetNodeAttribute(t_tree, "axis", m_cJointAxis);
60  if(NodeAttributeExists(t_tree, "limit")) {
61  m_bHasLimit = true;
62  CRange<CDegrees> cJointLimit;
63  GetNodeAttribute(t_tree, "limit", cJointLimit);
64  m_uLimit.Revolute = CRange<CRadians>(ToRadians(cJointLimit.GetMin()),
65  ToRadians(cJointLimit.GetMax()));
66  }
67  }
68  else if(strJointType == "spherical") {
69  m_eType = EType::SPHERICAL;
70  }
71  else {
72  THROW_ARGOSEXCEPTION("Joint type \"" << strJointType << "\" is not implemented");
73  }
74  /* parse the parent node */
75  TConfigurationNode& tJointParentNode = GetNode(t_tree, "parent");
76  std::string strJointParentLink;
77  GetNodeAttribute(tJointParentNode, "link", strJointParentLink);
78  m_pcParentLink = &(cLinkEquippedEntity.GetLink(strJointParentLink));
79  GetNodeAttributeOrDefault(tJointParentNode,
80  "position",
81  m_cParentLinkJointPosition,
82  m_cParentLinkJointPosition);
83  GetNodeAttributeOrDefault(tJointParentNode,
84  "orientation",
85  m_cParentLinkJointOrientation,
86  m_cParentLinkJointOrientation);
87  /* parse the child node */
88  TConfigurationNode& tJointChildNode = GetNode(t_tree, "child");
89  std::string strJointChildLink;
90  GetNodeAttribute(tJointChildNode, "link", strJointChildLink);
91  m_pcChildLink = &(cLinkEquippedEntity.GetLink(strJointChildLink));
92  GetNodeAttributeOrDefault(tJointChildNode,
93  "position",
94  m_cChildLinkJointPosition,
95  m_cChildLinkJointPosition);
96  GetNodeAttributeOrDefault(tJointChildNode,
97  "orientation",
98  m_cChildLinkJointOrientation,
99  m_cChildLinkJointOrientation);
100  }
101  catch(CARGoSException& ex) {
102  THROW_ARGOSEXCEPTION_NESTED("Error while initializing joint entity", ex);
103  }
104  }
105 
106  /****************************************/
107  /****************************************/
108 
110 
111  /****************************************/
112  /****************************************/
113 
114 }
A 3D vector class.
Definition: vector3.h:29
CPrototypeLinkEntity & GetLink(UInt32 un_index)
void GetNodeAttributeOrDefault(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer, const T &t_default)
Returns the value of a node's attribute, or the passed default value.
T GetMax() const
Definition: range.h:48
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
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.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
T GetMin() const
Definition: range.h:32
Basic class for an entity that contains other entities.
bool NodeAttributeExists(TConfigurationNode &t_node, const std::string &str_attribute)
Returns true if the specified attribute of a node exists.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
The exception that wraps all errors in ARGoS.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: entity.cpp:40
CRadians ToRadians(const CDegrees &c_degrees)
Converts CDegrees to CRadians.
Definition: angles.h:498
CComposableEntity & GetParent()
Returns this entity's parent.
Definition: entity.cpp:91
REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CComposableEntity)
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.
CPrototypeJointEntity(CComposableEntity *pc_parent)