dynamics2d_velocity_control.cpp
Go to the documentation of this file.
1 
8 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_engine.h>
9 
10 namespace argos {
11 
12  /****************************************/
13  /****************************************/
14 
16  Real f_max_force,
17  Real f_max_torque,
18  TConfigurationNode* t_node) :
19  m_cDyn2DEngine(c_engine),
20  m_ptControlBody(nullptr),
21  m_ptControlledBody(nullptr),
22  m_ptLinearConstraint(nullptr),
23  m_ptAngularConstraint(nullptr),
24  m_fMaxForce(f_max_force),
25  m_fMaxTorque(f_max_torque) {
26  m_ptControlBody = cpBodyNew(INFINITY, INFINITY);
27  if(t_node &&
28  NodeExists(*t_node, "dynamics2d")) {
29  TConfigurationNode& tNode = GetNode(*t_node, "dynamics2d");
30  if(NodeExists(tNode, "differential_steering")) {
31  TConfigurationNode& tDiffSteer = GetNode(tNode, "differential_steering");
32  GetNodeAttributeOrDefault(tDiffSteer, "max_force", m_fMaxForce, m_fMaxForce);
33  GetNodeAttributeOrDefault(tDiffSteer, "max_torque", m_fMaxTorque, m_fMaxTorque);
34  }
35  }
36  }
37 
38  /****************************************/
39  /****************************************/
40 
42  cpBodyFree(m_ptControlBody);
43  }
44 
45  /****************************************/
46  /****************************************/
47 
48  void CDynamics2DVelocityControl::AttachTo(cpBody* pt_body) {
49  /* If we are already controlling a body, detach from it first */
50  if(m_ptControlledBody != nullptr) {
51  Detach();
52  }
53  /* Set the wanted body as the new controlled one */
54  m_ptControlledBody = pt_body;
55  /* Add linear constraint */
57  cpSpaceAddConstraint(m_cDyn2DEngine.GetPhysicsSpace(),
58  cpPivotJointNew2(m_ptControlledBody,
60  cpvzero,
61  cpvzero));
62  m_ptLinearConstraint->maxBias = 0.0f; /* disable joint correction */
63  m_ptLinearConstraint->maxForce = m_fMaxForce; /* limit the dragging force */
64  /* Add angular constraint */
66  cpSpaceAddConstraint(m_cDyn2DEngine.GetPhysicsSpace(),
67  cpGearJointNew(m_ptControlledBody,
69  0.0f,
70  1.0f));
71  m_ptAngularConstraint->maxBias = 0.0f; /* disable joint correction */
72  m_ptAngularConstraint->maxForce = m_fMaxTorque; /* limit the torque */
73  }
74 
75  /****************************************/
76  /****************************************/
77 
79  if(m_ptControlledBody != nullptr) {
80  /* Remove constraints */
81  cpSpaceRemoveConstraint(m_cDyn2DEngine.GetPhysicsSpace(), m_ptLinearConstraint);
82  cpSpaceRemoveConstraint(m_cDyn2DEngine.GetPhysicsSpace(), m_ptAngularConstraint);
83  /* Free memory up */
84  cpConstraintFree(m_ptLinearConstraint);
85  cpConstraintFree(m_ptAngularConstraint);
86  /* Erase pointer to controlled body */
87  m_ptControlledBody = nullptr;
88  }
89  }
90 
91  /****************************************/
92  /****************************************/
93 
95  m_ptControlBody->v.x = 0;
96  m_ptControlBody->v.y = 0;
97  m_ptControlBody->w = 0;
98  }
99 
100  /****************************************/
101  /****************************************/
102 
104  return CVector2(m_ptControlledBody->v.x,
105  m_ptControlledBody->v.y);
106  }
107 
108  /****************************************/
109  /****************************************/
110 
112  m_ptControlBody->v.x = c_velocity.GetX();
113  m_ptControlBody->v.y = c_velocity.GetY();
114  }
115 
116  /****************************************/
117  /****************************************/
118 
120  return m_ptControlBody->w;
121  }
122 
123  /****************************************/
124  /****************************************/
125 
127  m_ptControlBody->w = f_velocity;
128  }
129 
130  /****************************************/
131  /****************************************/
132 
133 }
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
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.
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.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
bool NodeExists(TConfigurationNode &t_node, const std::string &str_tag)
Given a tree root node, returns true if one of its child nodes has the wanted name.
A 2D vector class.
Definition: vector2.h:27
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector2.h:110
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector2.h:94
CDynamics2DVelocityControl(CDynamics2DEngine &c_engine, Real f_max_force, Real f_max_torque, TConfigurationNode *t_node=NULL)
void SetLinearVelocity(const CVector2 &c_velocity)