physx_miniquadrotor_model.cpp
Go to the documentation of this file.
1 
8 #include <argos3/plugins/robots/mini-quadrotor/simulator/miniquadrotor_entity.h>
9 #include <argos3/plugins/simulator/entities/rotor_equipped_entity.h>
10 
11 namespace argos {
12 
13  /****************************************/
14  /****************************************/
15 
16  /*
17  * The parameter values are taken from data provided by Kumar's group directly
18  */
19  static const Real ARM_LENGTH = 0.063f;
20  static const Real ARM_HALF_LENGTH = ARM_LENGTH * 0.5f;
21  static const Real PROPELLER_LENGTH = 0.082f;
22  static const Real PROPELLER_HALF_LENGTH = PROPELLER_LENGTH * 0.5f;
23  static const Real BODY_HALF_WIDTH = ARM_HALF_LENGTH + PROPELLER_HALF_LENGTH;
24  static const Real BODY_HEIGHT = 0.015f;
25  static const Real BODY_HALF_HEIGHT = BODY_HEIGHT * 0.5f;
26  static const Real BODY_MASS = 0.06736f;
27  static const Real UPLIFT_COEFFICIENT = 1.9985e-9;
28  static const Real DRAG_COEFFICIENT = 4.737e-12;
29 
30  static const physx::PxVec3 POSITION_ERROR_COEFF(6.61f, 6.61f, 72.0f); // unused
31  static const physx::PxVec3 VELOCITY_ERROR_COEFF(5.14f, 5.14f, 24.0f); // unused
32 
33  static const physx::PxTransform PITCH_ARM_POSE(physx::PxQuat(physx::PxHalfPi, physx::PxVec3(0.0f, 0.0f, 1.0f)));
34  static const physx::PxVec3 INERTIA_TENSOR_DIAGONAL(2.32e-3, 2.32e-3, 4e-3);
35  static const physx::PxMat33 INERTIA_TENSOR(physx::PxMat33::createDiagonal(INERTIA_TENSOR_DIAGONAL));
36  static const physx::PxMat33 INERTIA_TENSOR_INVERSE(INERTIA_TENSOR.getInverse());
37 
38  /****************************************/
39  /****************************************/
40 
42  CMiniQuadrotorEntity& c_entity) :
43  CPhysXSingleBodyObjectModel(c_engine, c_entity),
44  m_cMiniQuadrotorEntity(c_entity) {
45  /* Calculate base center */
46  SetARGoSReferencePoint(physx::PxVec3(0.0f, 0.0f, -BODY_HALF_HEIGHT));
47  /* Get position and orientation in this engine's representation */
48  physx::PxVec3 cPos;
49  CVector3ToPxVec3(GetEmbodiedEntity().GetOriginAnchor().Position, cPos);
50  physx::PxQuat cOrient;
51  CQuaternionToPxQuat(GetEmbodiedEntity().GetOriginAnchor().Orientation, cOrient);
52  /* Create the transform
53  * 1. a translation up by half body height
54  * 2. a rotation around the base
55  * 3. a translation to the final position
56  */
57  physx::PxTransform cTranslation1(physx::PxVec3(0.0f, 0.0f, BODY_HALF_HEIGHT));
58  physx::PxTransform cRotation(cOrient);
59  physx::PxTransform cTranslation2(cPos);
60  physx::PxTransform cFinalTrans = cTranslation2 * cRotation * cTranslation1;
61  /* Create the capsule geometry for the arms */
62  physx::PxCapsuleGeometry cArmGeometry(BODY_HALF_HEIGHT,
63  BODY_HALF_WIDTH);
64  /* Create the body in its initial position and orientation */
65  m_pcBody = GetPhysXEngine().GetPhysics().createRigidDynamic(cFinalTrans);
66  /* Enable CCD on the body */
67  m_pcBody->setRigidBodyFlag(physx::PxRigidBodyFlag::eENABLE_CCD, true);
68  /* Create the shape for the roll arm */
69  physx::PxShape* pcRollArmShape =
70  m_pcBody->createShape(cArmGeometry,
71  GetPhysXEngine().GetDefaultMaterial());
72  pcRollArmShape->userData = this;
73  /* Create the shape for the pitch arm */
74  physx::PxShape* pcPitchArmShape =
75  m_pcBody->createShape(cArmGeometry,
76  GetPhysXEngine().GetDefaultMaterial(),
77  PITCH_ARM_POSE);
78  pcPitchArmShape->userData = this;
79  /* Set body mass and inertia tensor */
80  m_pcBody->setMass(BODY_MASS);
81  m_pcBody->setMassSpaceInertiaTensor(INERTIA_TENSOR_DIAGONAL);
82  /* Add body to the scene */
83  GetPhysXEngine().GetScene().addActor(*m_pcBody);
84  /* Setup the body model data */
85  SetupBody(m_pcBody);
86  /* Now we can calculate the bounding box */
87  CalculateBoundingBox();
88  }
89 
90  /****************************************/
91  /****************************************/
92 
94  /* Get desired rotor velocities */
95  const Real* pfRotorVel =
96  m_cMiniQuadrotorEntity.GetRotorEquippedEntity().GetRotorVelocities();
97  /* Calculate the squares */
98  Real pfSquareRotorVel[4] = {
99  pfRotorVel[0] * pfRotorVel[0],
100  pfRotorVel[1] * pfRotorVel[1],
101  pfRotorVel[2] * pfRotorVel[2],
102  pfRotorVel[3] * pfRotorVel[3]
103  };
104  /* Calculate uplift-related control input */
105  Real fUpliftInput =
106  UPLIFT_COEFFICIENT *
107  ((pfSquareRotorVel[0]) +
108  (pfSquareRotorVel[1]) +
109  (pfSquareRotorVel[2]) +
110  (pfSquareRotorVel[3]));
111  /* Calculate torque-related control inputs */
112  physx::PxVec3 cTorqueInputs(
113  UPLIFT_COEFFICIENT * ARM_HALF_LENGTH * (pfSquareRotorVel[0] - pfSquareRotorVel[2]),
114  UPLIFT_COEFFICIENT * ARM_HALF_LENGTH * (pfSquareRotorVel[1] - pfSquareRotorVel[3]),
115  DRAG_COEFFICIENT * (pfSquareRotorVel[0] - pfSquareRotorVel[1] + pfSquareRotorVel[2] - pfSquareRotorVel[3]));
116  /* Apply uplift */
117  physx::PxRigidBodyExt::addLocalForceAtLocalPos(
118  *m_pcBody,
119  physx::PxVec3(0.0f, 0.0f, fUpliftInput),
120  physx::PxVec3(0.0f));
121  /* Apply rotational moment */
122  physx::PxVec3 cTorque = (-m_pcBody->getAngularVelocity()).cross(INERTIA_TENSOR * m_pcBody->getAngularVelocity()) + cTorqueInputs;
123  m_pcBody->addTorque(cTorque);
124  }
125 
126  /****************************************/
127  /****************************************/
128 
130 
131  /****************************************/
132  /****************************************/
133 
134 }
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
REGISTER_STANDARD_PHYSX_OPERATIONS_ON_ENTITY(CEPuckEntity, CPhysXEPuckModel)
CRotorEquippedEntity & GetRotorEquippedEntity()
CPhysXMiniQuadrotorModel(CPhysXEngine &c_engine, CMiniQuadrotorEntity &c_entity)
const Real * GetRotorVelocities() const