8 #include <argos3/plugins/robots/e-puck/simulator/epuck_entity.h>
15 static const Real EPUCK_INTERWHEEL_DISTANCE = 0.053f;
16 static const Real EPUCK_WHEEL_RADIUS = 0.0205f;
17 static const Real EPUCK_WHEEL_THICKNESS = 0.01f;
18 static const Real EPUCK_WHEEL_MASS = 0.05f;
20 static const Real EPUCK_CHASSIS_LENGTH = 0.055f;
21 static const Real EPUCK_CHASSIS_WIDTH = (EPUCK_INTERWHEEL_DISTANCE - EPUCK_WHEEL_THICKNESS) * 0.9f;
22 static const Real EPUCK_CHASSIS_HEIGHT = EPUCK_WHEEL_RADIUS * 2.0f;
23 static const Real EPUCK_CHASSIS_ELEVATION = 0.005f;
24 static const Real EPUCK_CHASSIS_MASS = 0.3f;
26 static const Real EPUCK_BOARD_RADIUS = 0.035f;
27 static const Real EPUCK_BOARD_HEIGHT = 0.03f;
39 CPhysXMultiBodyObjectModel(c_engine, c_entity, 3),
40 m_fCurrentWheelVelocity(c_entity.GetWheeledEntity().GetWheelVelocities()),
43 EPUCK_INTERWHEEL_DISTANCE,
45 EPUCK_WHEEL_THICKNESS,
47 physx::PxVec3(EPUCK_CHASSIS_LENGTH,
49 EPUCK_CHASSIS_HEIGHT),
50 EPUCK_CHASSIS_ELEVATION,
53 physx::PxTransform cBodyTrans;
54 CVector3ToPxVec3(GetEmbodiedEntity().GetOriginAnchor().Position, cBodyTrans.p);
55 CQuaternionToPxQuat(GetEmbodiedEntity().GetOriginAnchor().Orientation, cBodyTrans.q);
60 physx::PxConvexMeshGeometry* pcBoardGeometry =
61 CreateCylinderGeometry(c_engine,
65 physx::PxShape* pcBoardShape =
66 m_cDiffDrive.GetMainBodyActor().createShape(*pcBoardGeometry,
67 GetPhysXEngine().GetDefaultMaterial());
68 pcBoardShape->userData =
this;
70 pcBoardShape->setLocalPose(
71 physx::PxTransform(0.0f,
73 EPUCK_CHASSIS_ELEVATION +
74 EPUCK_CHASSIS_HEIGHT +
75 EPUCK_BOARD_HEIGHT * 0.5f));
77 m_cDiffDrive.SetGlobalPose(cBodyTrans);
79 CalculateBoundingBox();
81 delete pcBoardGeometry;
83 RegisterAnchorMethod(GetEmbodiedEntity().GetOriginAnchor(),
92 CPhysXMultiBodyObjectModel::Reset();
94 m_cDiffDrive.SetTargetWheelLinearVelocity(0.0f, 0.0f);
104 m_cDiffDrive.SetTargetWheelLinearVelocity
110 m_cDiffDrive.SetTargetWheelLinearVelocity(0.0f, 0.0f);
119 physx::PxTransform cBodyTrans =
120 m_cDiffDrive.GetMainBodyActor().getGlobalPose() *
121 m_cDiffDrive.GetMainBodyOffset().getInverse();
123 PxVec3ToCVector3(cBodyTrans.p, s_anchor.
Position);
125 PxQuatToCQuaternion(cBodyTrans.q, s_anchor.
Orientation);
126 physx::PxReal fLeftAngle;
127 physx::PxReal fRightAngle;
128 m_cDiffDrive.GetCurrentWheelAngles(fLeftAngle, fRightAngle);
129 DEBUG(
"Current wheel rotation: %f, %f\n", fLeftAngle, fRightAngle);
130 physx::PxReal fLeftVelocity;
131 physx::PxReal fRightVelocity;
132 m_cDiffDrive.GetCurrentWheelLinearVelocity(fLeftVelocity, fRightVelocity);
133 DEBUG(
"Current wheel speed: %f, %f\n", fLeftVelocity, fRightVelocity);
134 physx::PxReal fLeftTargetVelocity;
135 physx::PxReal fRightTargetVelocity;
136 m_cDiffDrive.GetTargetWheelLinearVelocity(fLeftTargetVelocity, fRightTargetVelocity);
137 DEBUG(
"Target wheel speed: %f, %f\n\n", fLeftTargetVelocity, fRightTargetVelocity);