/
dynamics2d_kilobot_model.cpp
149 lines (127 loc) · 6.1 KB
/
dynamics2d_kilobot_model.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
/**
* @file <argos3/plugins/robots/kilobot/simulator/dynamics2d_kilobot_model.cpp>
*
* @author Carlo Pinciroli - <ilpincy@gmail.com>
* @author Vito Trianni - <vito.trianni@istc.cnr.it>
*/
#include "dynamics2d_kilobot_model.h"
#include "kilobot_measures.h"
#include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_engine.h>
namespace argos {
/****************************************/
/****************************************/
static const Real KILOBOT_MAX_FORCE = 0.001f;
static const Real KILOBOT_MAX_TORQUE = 0.001f;
static const Real KILOBOT_FRICTION = 2.5f;
enum KILOBOT_WHEELS {
KILOBOT_LEFT_WHEEL = 0,
KILOBOT_RIGHT_WHEEL = 1
};
/****************************************/
/****************************************/
CDynamics2DKilobotModel::CDynamics2DKilobotModel(CDynamics2DEngine& c_engine,
CKilobotEntity& c_entity) :
CDynamics2DSingleBodyObjectModel(c_engine, c_entity),
m_cKilobotEntity(c_entity),
m_cWheeledEntity(m_cKilobotEntity.GetWheeledEntity()),
m_cDiffSteering(c_engine,
KILOBOT_MAX_FORCE,
KILOBOT_MAX_TORQUE,
KILOBOT_INTERPIN_DISTANCE,
c_entity.GetConfigurationNode()),
m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()) {
/* Parse the XML file to check if friction was specified */
cpFloat fFriction = KILOBOT_FRICTION;
if(c_entity.GetConfigurationNode() &&
NodeExists(*c_entity.GetConfigurationNode(), "dynamics2d")) {
TConfigurationNode& tDyn2D = GetNode(*c_entity.GetConfigurationNode(), "dynamics2d");
GetNodeAttributeOrDefault(tDyn2D, "friction", fFriction, fFriction);
}
/* Create the actual body with initial position and orientation */
cpBody* ptBody =
cpSpaceAddBody(GetDynamics2DEngine().GetPhysicsSpace(),
cpBodyNew(KILOBOT_MASS,
cpMomentForCircle(KILOBOT_MASS,
0.0f,
KILOBOT_RADIUS + KILOBOT_RADIUS,
cpv(KILOBOT_ECCENTRICITY,0))));
const CVector3& cPosition = GetEmbodiedEntity().GetOriginAnchor().Position;
ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
CRadians cXAngle, cYAngle, cZAngle;
GetEmbodiedEntity().GetOriginAnchor().Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(ptBody, cZAngle.GetValue());
/* Create the actual body shape */
cpShape* ptShape =
cpSpaceAddShape(GetDynamics2DEngine().GetPhysicsSpace(),
cpCircleShapeNew(ptBody,
KILOBOT_RADIUS,
cpv(KILOBOT_ECCENTRICITY,0)));
ptShape->e = 0.0; // No elasticity
ptShape->u = fFriction; // Friction
/* Constrain the body to follow the diff steering control */
m_cDiffSteering.AttachTo(ptBody);
/* Set the body so that the default methods work as expected */
SetBody(ptBody, KILOBOT_HEIGHT);
/* Set the anchor updaters */
RegisterAnchorMethod<CDynamics2DKilobotModel>(
GetEmbodiedEntity().GetAnchor("light"),
&CDynamics2DKilobotModel::UpdateLightAnchor);
RegisterAnchorMethod<CDynamics2DKilobotModel>(
GetEmbodiedEntity().GetAnchor("comm"),
&CDynamics2DKilobotModel::UpdateCommAnchor);
}
/****************************************/
/****************************************/
CDynamics2DKilobotModel::~CDynamics2DKilobotModel() {
m_cDiffSteering.Detach();
}
/****************************************/
/****************************************/
void CDynamics2DKilobotModel::Reset() {
CDynamics2DSingleBodyObjectModel::Reset();
m_cDiffSteering.Reset();
}
/****************************************/
/****************************************/
void CDynamics2DKilobotModel::UpdateFromEntityStatus() {
/* Do we want to move? */
if((m_fCurrentWheelVelocity[KILOBOT_LEFT_WHEEL] != 0.0f) ||
(m_fCurrentWheelVelocity[KILOBOT_RIGHT_WHEEL] != 0.0f)) {
m_cDiffSteering.SetWheelVelocity(m_fCurrentWheelVelocity[KILOBOT_LEFT_WHEEL],
m_fCurrentWheelVelocity[KILOBOT_RIGHT_WHEEL]);
}
else {
/* No, we don't want to move - zero all speeds */
m_cDiffSteering.Reset();
}
}
/****************************************/
/****************************************/
void CDynamics2DKilobotModel::UpdateLightAnchor(SAnchor& s_anchor) {
/* Start in origin, put anchor in offset */
s_anchor.Position = s_anchor.OffsetPosition;
/* Rotate anchor by body orientation in world */
s_anchor.Orientation.FromAngleAxis(CRadians(GetBody()->a), CVector3::Z);
s_anchor.Position.Rotate(s_anchor.Orientation);
/* Translate anchor by body position in world */
s_anchor.Position.SetX(s_anchor.Position.GetX() + GetBody()->p.x);
s_anchor.Position.SetY(s_anchor.Position.GetY() + GetBody()->p.y);
}
/****************************************/
/****************************************/
void CDynamics2DKilobotModel::UpdateCommAnchor(SAnchor& s_anchor) {
/* Start in origin, put anchor in offset */
s_anchor.Position = s_anchor.OffsetPosition;
/* Rotate anchor by body orientation in world */
s_anchor.Orientation.FromAngleAxis(CRadians(GetBody()->a), CVector3::Z);
s_anchor.Position.Rotate(s_anchor.Orientation);
/* Translate anchor by body position in world */
s_anchor.Position.SetX(s_anchor.Position.GetX() + GetBody()->p.x);
s_anchor.Position.SetY(s_anchor.Position.GetY() + GetBody()->p.y);
}
/****************************************/
/****************************************/
REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY(CKilobotEntity, CDynamics2DKilobotModel);
/****************************************/
/****************************************/
}