9 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_gripping.h>
10 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_engine.h>
18 static const Real PD_P_CONSTANT = 0.4;
19 static const Real PD_D_CONSTANT = 0.2;
21 static const Real FOOTBOT_RADIUS = 0.085036758f;
22 static const Real FOOTBOT_INTERWHEEL_DISTANCE = 0.14f;
23 static const Real FOOTBOT_HEIGHT = 0.146899733f;
25 static const Real FOOTBOT_MAX_FORCE = 15.f;
26 static const Real FOOTBOT_MAX_TORQUE = 150.f;
46 m_cFootBotEntity(c_entity),
47 m_cWheeledEntity(m_cFootBotEntity.GetWheeledEntity()),
48 m_cGripperEntity(c_entity.GetGripperEquippedEntity()),
49 m_cDiffSteering(c_engine,
52 FOOTBOT_INTERWHEEL_DISTANCE,
53 c_entity.GetConfigurationNode()),
57 m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()),
58 m_unLastTurretMode(m_cFootBotEntity.GetTurretEntity().GetMode()) {
59 RegisterAnchorMethod<CDynamics2DFootBotModel>(
62 RegisterAnchorMethod<CDynamics2DFootBotModel>(
65 RegisterAnchorMethod<CDynamics2DFootBotModel>(
72 cpMomentForCircle(m_fMass,
74 FOOTBOT_RADIUS + FOOTBOT_RADIUS,
77 m_ptActualBaseBody->p = cpv(cPosition.
GetX(), cPosition.
GetY());
80 cpBodySetAngle(m_ptActualBaseBody, cZAngle.GetValue());
84 cpCircleShapeNew(m_ptActualBaseBody,
87 m_ptBaseShape->e = 0.0;
88 m_ptBaseShape->u = 0.7;
93 m_cDiffSteering.
AttachTo(m_ptActualBaseBody);
95 AddBody(m_ptActualBaseBody, cpvzero, 0, FOOTBOT_HEIGHT);
97 m_ptActualGripperBody =
99 cpBodyNew(m_fMass / 20.0,
100 cpMomentForCircle(m_fMass,
102 FOOTBOT_RADIUS + FOOTBOT_RADIUS,
104 m_ptActualGripperBody->p = cpv(cPosition.
GetX(), cPosition.
GetY());
105 cpBodySetAngle(m_ptActualGripperBody,
109 cpShape* ptGripperShape =
111 cpCircleShapeNew(m_ptActualGripperBody,
113 cpv(FOOTBOT_RADIUS, 0.0f)));
118 m_ptBaseGripperLinearMotion =
120 cpPivotJointNew2(m_ptActualBaseBody,
121 m_ptActualGripperBody,
124 m_ptBaseGripperAngularMotion = cpSpaceAddConstraint(
GetDynamics2DEngine().GetPhysicsSpace(),
125 cpGearJointNew(m_ptActualBaseBody,
126 m_ptActualGripperBody,
129 m_ptBaseGripperAngularMotion->maxBias = 0.0f;
130 m_ptBaseGripperAngularMotion->maxForce = FOOTBOT_MAX_TORQUE;
132 AddBody(m_ptActualGripperBody, cpvzero, 0, FOOTBOT_HEIGHT);
136 TurretActiveToPassive();
145 delete m_pcGrippable;
146 switch(m_unLastTurretMode) {
149 cpSpaceRemoveConstraint(
GetDynamics2DEngine().GetPhysicsSpace(), m_ptBaseGripperLinearMotion);
150 cpSpaceRemoveConstraint(
GetDynamics2DEngine().GetPhysicsSpace(), m_ptBaseGripperAngularMotion);
151 cpConstraintFree(m_ptBaseGripperLinearMotion);
152 cpConstraintFree(m_ptBaseGripperAngularMotion);
156 cpSpaceRemoveConstraint(
GetDynamics2DEngine().GetPhysicsSpace(), m_ptBaseGripperLinearMotion);
157 cpSpaceRemoveConstraint(
GetDynamics2DEngine().GetPhysicsSpace(), m_ptGripperControlAngularMotion);
158 cpConstraintFree(m_ptBaseGripperLinearMotion);
159 cpConstraintFree(m_ptGripperControlAngularMotion);
160 cpBodyFree(m_ptControlGripperBody);
184 m_cDiffSteering.
Reset();
191 TurretActiveToPassive();
223 m_cDiffSteering.
Reset();
235 switch(m_unLastTurretMode) {
241 TurretPassiveToActive();
253 TurretActiveToPassive();
265 switch(m_unLastTurretMode) {
272 CRadians(m_ptActualBaseBody->a))).GetValue();
273 m_ptControlGripperBody->w =
275 (PD_P_CONSTANT * fCurRotErr +
277 m_fPreviousTurretAngleError = fCurRotErr;
281 m_ptControlGripperBody->w =
289 m_ptBaseGripperAngularMotion->maxForce = 0.0001f;
292 m_ptBaseGripperAngularMotion->maxForce = FOOTBOT_MAX_TORQUE;
301 void CDynamics2DFootBotModel::TurretPassiveToActive() {
303 cpSpaceRemoveConstraint(
GetDynamics2DEngine().GetPhysicsSpace(), m_ptBaseGripperAngularMotion);
304 cpConstraintFree(m_ptBaseGripperAngularMotion);
306 m_ptControlGripperBody = cpBodyNew(INFINITY, INFINITY);
308 m_ptGripperControlAngularMotion = cpSpaceAddConstraint(
GetDynamics2DEngine().GetPhysicsSpace(),
309 cpGearJointNew(m_ptActualGripperBody,
310 m_ptControlGripperBody,
313 m_ptGripperControlAngularMotion->maxBias = 0.0f;
314 m_ptGripperControlAngularMotion->maxForce = FOOTBOT_MAX_TORQUE;
320 void CDynamics2DFootBotModel::TurretActiveToPassive() {
322 cpSpaceRemoveConstraint(
GetDynamics2DEngine().GetPhysicsSpace(), m_ptGripperControlAngularMotion);
323 cpConstraintFree(m_ptGripperControlAngularMotion);
325 cpBodyFree(m_ptControlGripperBody);
327 m_ptBaseGripperAngularMotion = cpSpaceAddConstraint(
GetDynamics2DEngine().GetPhysicsSpace(),
328 cpGearJointNew(m_ptActualBaseBody,
329 m_ptActualGripperBody,
332 m_ptBaseGripperAngularMotion->maxBias = 0.0f;
333 m_ptBaseGripperAngularMotion->maxForce = FOOTBOT_MAX_TORQUE;
Base class for object models with multiple bodies.
void AttachTo(cpBody *pt_body)
static Real GetInverseSimulationClockTick()
Returns the inverse of GetSimulationClockTick().
float Real
Collects all ARGoS code.
CRadians GetRotation() const
Real GetX() const
Returns the x coordinate of this vector.
CQuaternion OffsetOrientation
The initial orientation of the anchor wrt the body coordinate system.
bool IsGripping() const
Returns true if this gripper is gripping something.
Real GetY() const
Returns the y coordinate of this vector.
virtual void MoveTo(const CVector3 &c_position, const CQuaternion &c_orientation)
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
CDynamics2DFootBotModel(CDynamics2DEngine &c_engine, CFootBotEntity &c_entity)
It defines the basic type CRadians, used to store an angle value in radians.
virtual void AddBody(cpBody *pt_body, const cpVect &t_offset_pos, cpFloat t_offset_orient, Real f_height)
Adds a body.
bool IsLocked() const
Returns true if the gripper is locked.
CFootBotTurretEntity & GetTurretEntity()
virtual void MoveTo(const CVector3 &c_position, const CQuaternion &c_orientation)
Real GetDesiredRotationSpeed() const
void UpdateTurretAnchor(SAnchor &s_anchor)
CQuaternion & FromAngleAxis(const CRadians &c_angle, const CVector3 &c_vector)
void SetX(const Real f_x)
Sets the x coordinate of this vector.
An anchor related to the body of an entity.
CDynamics2DEngine & GetDynamics2DEngine()
Returns the dynamics 2D engine state.
void UpdatePerspectiveCameraAnchor(SAnchor &s_anchor)
void UpdateOriginAnchor(SAnchor &s_anchor)
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
void SetWheelVelocity(Real f_left_wheel, Real f_right_wheel)
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
static const CVector3 Z
The z axis.
CRadians NormalizedDifference(const CRadians &c_angle1, const CRadians &c_angle2)
Calculates the normalized difference between the given angles.
void SetY(const Real f_y)
Sets the y coordinate of this vector.
Real GetValue() const
Returns the value in radians.
const SBoundingBox & GetBoundingBox() const
Returns an axis-aligned box that contains the physics model.
virtual ~CDynamics2DFootBotModel()
CVector3 Position
The position of the anchor wrt the global coordinate system.
CVector3 OffsetPosition
The initial position of the anchor wrt the body coordinate system.
const SAnchor & GetAnchor(const std::string &str_id) const
Returns the wanted anchor as a const reference.
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.
The namespace containing all the ARGoS related code.
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
const CRadians & GetDesiredRotation() const
REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY(CEPuckEntity, CDynamics2DEPuckModel)
void DisableAnchor(const std::string &str_id)
Disables an anchor.
Real GetAngularVelocity() const
void EnableAnchor(const std::string &str_id)
Enables an anchor.
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.