pointmass3d_spiri_model.cpp
Go to the documentation of this file.
1 
7 #include "pointmass3d_spiri_model.h"
8 #include <argos3/core/utility/logging/argos_log.h>
9 #include <argos3/core/utility/math/cylinder.h>
10 #include <argos3/core/simulator/simulator.h>
11 #include <argos3/core/simulator/space/space.h>
12 #include <argos3/plugins/simulator/physics_engines/pointmass3d/pointmass3d_engine.h>
13 
14 namespace argos {
15 
16  static const Real BODY_HEIGHT = 0.090f;
17  static const Real BODY_SIDE = 0.470f;
18  static const Real BODY_DIAGONAL = BODY_SIDE * ::sqrt(2);
19  static const Real BODY_RADIUS = BODY_DIAGONAL / 2.0f;
20  static const Real BODY_MASS = 1.00f;
21  static const Real BODY_INERTIA = 0.01f;
22  static const CVector3 POS_K_P(20.0f, 20.0f, 20.0f);
23  static const CVector3 POS_K_D(10.0f, 10.0f, 10.0f);
24  static const Real YAW_K_P = 0.5f;
25  static const Real YAW_K_D = 0.1f;
26  static const CVector3 VEL_K_P(20.0f, 20.0f, 20.0f);
27  static const CVector3 VEL_K_D(10.0f, 10.0f, 10.0f);
28  static const Real ROT_K_P = 0.5f;
29  static const Real ROT_K_D = 0.1f;
30 
31  /****************************************/
32  /****************************************/
33 
34  CPointMass3DSpiriModel::CPointMass3DSpiriModel(CPointMass3DEngine& c_engine,
35  CSpiriEntity& c_spiri) :
36  CPointMass3DQuadRotorModel(c_engine,
37  c_spiri.GetEmbodiedEntity(),
38  c_spiri.GetQuadRotorEntity(),
39  BODY_HEIGHT,
40  BODY_RADIUS,
41  BODY_MASS,
42  BODY_INERTIA,
43  POS_K_P,
44  POS_K_D,
45  YAW_K_P,
46  YAW_K_D,
47  VEL_K_P,
48  VEL_K_D,
49  ROT_K_P,
50  ROT_K_D) {
51  /* Register anchor update methods */
52  RegisterAnchorMethod(GetEmbodiedEntity().GetAnchor("rab"),
53  &CPointMass3DSpiriModel::UpdateRABAnchor);
54  RegisterAnchorMethod(GetEmbodiedEntity().GetAnchor("camera"),
55  &CPointMass3DSpiriModel::UpdateCameraAnchor);
56  }
57 
58  /****************************************/
59  /****************************************/
60 
61  void CPointMass3DSpiriModel::UpdateRABAnchor(SAnchor& s_anchor) {
62  s_anchor.Orientation = GetEmbodiedEntity().GetOriginAnchor().Orientation;
63  s_anchor.Position = s_anchor.OffsetPosition;
64  s_anchor.Position += GetEmbodiedEntity().GetOriginAnchor().Position;
65  }
66 
67  /****************************************/
68  /****************************************/
69 
70  void CPointMass3DSpiriModel::UpdateCameraAnchor(SAnchor& s_anchor) {
71  s_anchor.Orientation = GetEmbodiedEntity().GetOriginAnchor().Orientation;
72  s_anchor.Orientation *= s_anchor.OffsetOrientation;
73  s_anchor.Position = s_anchor.OffsetPosition;
74  s_anchor.Position.Rotate(s_anchor.Orientation);
75  s_anchor.Position += GetEmbodiedEntity().GetOriginAnchor().Position;
76  }
77 
78  /****************************************/
79  /****************************************/
80 
81  REGISTER_STANDARD_POINTMASS3D_OPERATIONS_ON_ENTITY(CSpiriEntity, CPointMass3DSpiriModel);
82 
83  /****************************************/
84  /****************************************/
85 
86 }
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::REGISTER_STANDARD_POINTMASS3D_OPERATIONS_ON_ENTITY
REGISTER_STANDARD_POINTMASS3D_OPERATIONS_ON_ENTITY(CEyeBotEntity, CPointMass3DEyeBotModel)
argos::CEmbodiedEntity::GetOriginAnchor
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
Definition: embodied_entity.h:119
argos::SAnchor
An anchor related to the body of an entity.
Definition: physics_model.h:38
argos::SAnchor::Orientation
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
argos::SAnchor::Position
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
argos::SAnchor::OffsetPosition
CVector3 OffsetPosition
The initial position of the anchor wrt the body coordinate system.
Definition: physics_model.h:47
argos::CPhysicsModel::RegisterAnchorMethod
void RegisterAnchorMethod(const SAnchor &s_anchor, void(MODEL::*pt_method)(SAnchor &))
Registers an anchor method.
Definition: physics_model.h:341
argos::CPhysicsModel::GetEmbodiedEntity
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
Definition: physics_model.h:133
argos::CPointMass3DSpiriModel::CPointMass3DSpiriModel
CPointMass3DSpiriModel(CPointMass3DEngine &c_engine, CSpiriEntity &c_spiri)
Definition: pointmass3d_spiri_model.cpp:34