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>
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;
37 c_spiri.GetEmbodiedEntity(),
38 c_spiri.GetQuadRotorEntity(),
53 &CPointMass3DSpiriModel::UpdateRABAnchor);
55 &CPointMass3DSpiriModel::UpdateCameraAnchor);
61 void CPointMass3DSpiriModel::UpdateRABAnchor(
SAnchor& s_anchor) {
70 void CPointMass3DSpiriModel::UpdateCameraAnchor(SAnchor& s_anchor) {
72 s_anchor.Orientation *= s_anchor.OffsetOrientation;
73 s_anchor.Position = s_anchor.OffsetPosition;
74 s_anchor.Position.Rotate(s_anchor.Orientation);