From ff7c55df0c09cb7309d6279492afd23b8af6c107 Mon Sep 17 00:00:00 2001 From: Michael Allwright Date: Thu, 24 Oct 2019 11:03:15 +0200 Subject: [PATCH 1/4] Use CFactory to create a controller when ARGOS_DYNAMIC_LIBRARY_LOADING is set --- src/core/real_robot/real_robot.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/core/real_robot/real_robot.cpp b/src/core/real_robot/real_robot.cpp index f0ed8091..15791584 100644 --- a/src/core/real_robot/real_robot.cpp +++ b/src/core/real_robot/real_robot.cpp @@ -72,7 +72,11 @@ void CRealRobot::Init(const std::string& str_conf_fname, * Initialize the controller */ LOG << "[INFO] Controller type '" << strControllerTag << "', id '" << str_controller_id << "' initialization start" << std::endl; +#ifdef ARGOS_DYNAMIC_LIBRARY_LOADING + m_pcController = CFactory::New(strControllerTag); +#else m_pcController = ControllerMaker(strControllerTag); +#endif /* Set the controller id using the machine hostname */ char pchHostname[256]; pchHostname[255] = '\0'; From d705a9d1141582691e660f247e3fae726ccb6679 Mon Sep 17 00:00:00 2001 From: Michael Allwright Date: Tue, 5 Nov 2019 15:40:50 +0100 Subject: [PATCH 2/4] Fixed bug in the reset logic for CDynamics3DModel and derived classes. --- .../prototype/simulator/dynamics3d_prototype_model.cpp | 2 +- .../physics_engines/dynamics3d/dynamics3d_box_model.cpp | 4 ++-- .../dynamics3d/dynamics3d_cylinder_model.cpp | 4 ++-- .../physics_engines/dynamics3d/dynamics3d_engine.cpp | 3 ++- .../physics_engines/dynamics3d/dynamics3d_model.cpp | 4 ++-- .../dynamics3d/dynamics3d_multi_body_object_model.cpp | 6 ++++++ 6 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/plugins/robots/prototype/simulator/dynamics3d_prototype_model.cpp b/src/plugins/robots/prototype/simulator/dynamics3d_prototype_model.cpp index 3aadda69..35fe7fad 100644 --- a/src/plugins/robots/prototype/simulator/dynamics3d_prototype_model.cpp +++ b/src/plugins/robots/prototype/simulator/dynamics3d_prototype_model.cpp @@ -265,7 +265,7 @@ namespace argos { } } - /* With this call to reset, the multi-body model is configured and ready to be added */ + /* Finalize model with a reset */ Reset(); } diff --git a/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_box_model.cpp b/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_box_model.cpp index 20ccbb89..e5cc9720 100644 --- a/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_box_model.cpp +++ b/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_box_model.cpp @@ -59,8 +59,8 @@ namespace argos { m_ptrBody = std::make_shared(*this, &sAnchor, ptrShape, sData); /* Transfer the body to the base class */ m_vecBodies.push_back(m_ptrBody); - /* Synchronize with the entity in the space */ - UpdateEntityStatus(); + /* Finalize model with a reset */ + Reset(); } /****************************************/ diff --git a/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_cylinder_model.cpp b/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_cylinder_model.cpp index 408288ae..8799f121 100644 --- a/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_cylinder_model.cpp +++ b/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_cylinder_model.cpp @@ -59,8 +59,8 @@ namespace argos { m_ptrBody = std::make_shared(*this, &sAnchor, ptrShape, sData); /* Transfer the body to the base class */ m_vecBodies.push_back(m_ptrBody); - /* Synchronize with the entity in the space */ - UpdateEntityStatus(); + /* Finalize model with a reset */ + Reset(); } /****************************************/ diff --git a/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_engine.cpp b/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_engine.cpp index e6981e6d..95e9971f 100644 --- a/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_engine.cpp +++ b/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_engine.cpp @@ -80,8 +80,9 @@ namespace argos { ++itPlugin) { itPlugin->second->UnregisterModel(*itModel->second); } - /* Reset the model */ + /* Remove model from world */ itModel->second->RemoveFromWorld(m_cWorld); + /* Reset the model */ itModel->second->Reset(); } /* Run the destructors on bullet's components */ diff --git a/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_model.cpp b/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_model.cpp index c2c8581f..c5f760b5 100644 --- a/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_model.cpp +++ b/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_model.cpp @@ -146,11 +146,11 @@ namespace argos { /****************************************/ void CDynamics3DModel::UpdateEntityStatus() { - /* Update the anchor associated with each body before running the base class's UpdateEntityStatus - which updates the bounding box and origin anchor */ + /* Update the anchor associated with each body */ for(const std::shared_ptr& ptr_body : m_vecBodies) { ptr_body->UpdateAnchor(); } + /* Call CPhysicsModel::UpdateEntityStatus which updates the AABB and origin anchor */ CPhysicsModel::UpdateEntityStatus(); } diff --git a/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_multi_body_object_model.cpp b/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_multi_body_object_model.cpp index a04c8eea..4ef126f0 100644 --- a/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_multi_body_object_model.cpp +++ b/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_multi_body_object_model.cpp @@ -46,6 +46,12 @@ namespace argos { for(const std::shared_ptr& ptr_body : m_vecBodies) { ptr_body->Reset(); } + /* TODO. For the moment, we rely on the class that derives from + this model to call UpdateEntityStatus after the joints + have been configured. This is ugly and should be fixed + by moving the code for joints into this class */ + /* Synchronize with the entity in the space */ + //UpdateEntityStatus(); } /****************************************/ From 5786197ed161a1d257e916a31a10e21fc513abc3 Mon Sep 17 00:00:00 2001 From: Michael Allwright Date: Fri, 8 Nov 2019 16:21:45 +0100 Subject: [PATCH 3/4] Add a dynamics3d model for the e-puck robot --- src/plugins/robots/e-puck/CMakeLists.txt | 5 +- .../simulator/dynamics3d_epuck_model.cpp | 206 ++++++++++++++++++ .../e-puck/simulator/dynamics3d_epuck_model.h | 75 +++++++ 3 files changed, 285 insertions(+), 1 deletion(-) create mode 100644 src/plugins/robots/e-puck/simulator/dynamics3d_epuck_model.cpp create mode 100644 src/plugins/robots/e-puck/simulator/dynamics3d_epuck_model.h diff --git a/src/plugins/robots/e-puck/CMakeLists.txt b/src/plugins/robots/e-puck/CMakeLists.txt index b71655e7..a4ed78f7 100644 --- a/src/plugins/robots/e-puck/CMakeLists.txt +++ b/src/plugins/robots/e-puck/CMakeLists.txt @@ -8,6 +8,7 @@ set(ARGOS3_HEADERS_PLUGINS_ROBOTS_EPUCK_CONTROLINTERFACE if(ARGOS_BUILD_FOR_SIMULATOR) set(ARGOS3_HEADERS_PLUGINS_ROBOTS_EPUCK_SIMULATOR simulator/dynamics2d_epuck_model.h + simulator/dynamics3d_epuck_model.h # simulator/physx_epuck_model.h simulator/epuck_entity.h simulator/epuck_proximity_default_sensor.h) @@ -23,6 +24,7 @@ if(ARGOS_BUILD_FOR_SIMULATOR) set(ARGOS3_SOURCES_PLUGINS_ROBOTS_EPUCK ${ARGOS3_SOURCES_PLUGINS_ROBOTS_EPUCK} ${ARGOS3_HEADERS_PLUGINS_ROBOTS_EPUCK_SIMULATOR} + simulator/dynamics3d_epuck_model.cpp simulator/dynamics2d_epuck_model.cpp # simulator/physx_epuck_model.cpp simulator/epuck_entity.cpp @@ -45,7 +47,8 @@ endif(ARGOS_BUILD_FOR_SIMULATOR) add_library(argos3plugin_${ARGOS_BUILD_FOR}_epuck SHARED ${ARGOS3_SOURCES_PLUGINS_ROBOTS_EPUCK}) target_link_libraries(argos3plugin_${ARGOS_BUILD_FOR}_epuck argos3plugin_${ARGOS_BUILD_FOR}_genericrobot - argos3plugin_${ARGOS_BUILD_FOR}_dynamics2d) + argos3plugin_${ARGOS_BUILD_FOR}_dynamics2d + argos3plugin_${ARGOS_BUILD_FOR}_dynamics3d) # argos3plugin_${ARGOS_BUILD_FOR}_physx) if(ARGOS_BUILD_FOR_SIMULATOR AND ARGOS_COMPILE_QTOPENGL) target_link_libraries(argos3plugin_${ARGOS_BUILD_FOR}_epuck argos3plugin_${ARGOS_BUILD_FOR}_qtopengl) diff --git a/src/plugins/robots/e-puck/simulator/dynamics3d_epuck_model.cpp b/src/plugins/robots/e-puck/simulator/dynamics3d_epuck_model.cpp new file mode 100644 index 00000000..b5adcd54 --- /dev/null +++ b/src/plugins/robots/e-puck/simulator/dynamics3d_epuck_model.cpp @@ -0,0 +1,206 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#include "dynamics3d_epuck_model.h" + +#include +#include + +#include +#include + +namespace argos { + + /****************************************/ + /****************************************/ + + CDynamics3DEPuckModel::CDynamics3DEPuckModel(CDynamics3DEngine& c_engine, + CEPuckEntity& c_epuck) : + /* technically, the CDynamics3DMultiBodyObjectModel should be initialized with 7 children + links, however, setting it to 7 makes the epuck less stable for reasons. */ + CDynamics3DMultiBodyObjectModel(c_engine, c_epuck, 3, false), + m_cWheeledEntity(c_epuck.GetWheeledEntity()) { + /* get the required collision shapes */ + std::shared_ptr ptrBodyShape = + CDynamics3DShapeManager::RequestBox(m_cBodyHalfExtents); + std::shared_ptr ptrWheelShape = + CDynamics3DShapeManager::RequestCylinder(m_cWheelHalfExtents); + /* calculate the inertia of the collision objects */ + btVector3 cBodyInertia; + btVector3 cWheelInertia; + ptrBodyShape->calculateLocalInertia(m_fBodyMass, cBodyInertia); + ptrWheelShape->calculateLocalInertia(m_fWheelMass, cWheelInertia); + /* calculate a btTransform that moves us from the global coordinate system to the + local coordinate system */ + const SAnchor& sOriginAnchor = c_epuck.GetEmbodiedEntity().GetOriginAnchor(); + const CQuaternion& cOrientation = sOriginAnchor.Orientation; + const CVector3& cPosition = sOriginAnchor.Position; + const btTransform& cStartTransform = btTransform( + btQuaternion(cOrientation.GetX(), + cOrientation.GetZ(), + -cOrientation.GetY(), + cOrientation.GetW()), + btVector3(cPosition.GetX(), + cPosition.GetZ(), + -cPosition.GetY())); + /* create a CAbstractBody::SData structure for each body */ + CAbstractBody::SData sBodyData( + cStartTransform * m_cBodyOffset, + m_cBodyGeometricOffset, + cBodyInertia, + m_fBodyMass, + GetEngine().GetDefaultFriction()); + CAbstractBody::SData sLeftWheelData( + cStartTransform * m_cLeftWheelOffset, + m_cWheelGeometricOffset, + cWheelInertia, + m_fWheelMass, + m_fWheelFriction); + CAbstractBody::SData sRightWheelData( + cStartTransform * m_cRightWheelOffset, + m_cWheelGeometricOffset, + cWheelInertia, + m_fWheelMass, + m_fWheelFriction); + /* create an anchor for the body (not strictly necessary but easier than + overloading CDynamics3DMultiBodyObjectModel::UpdateOriginAnchor) */ + SAnchor* psBodyAnchor = &c_epuck.GetEmbodiedEntity().AddAnchor("body", {0.0, 0.0, 0.00125}); + /* create the bodies */ + m_ptrBody = std::make_shared(*this, psBodyAnchor, ptrBodyShape, sBodyData); + m_ptrLeftWheel = std::make_shared(*this, 0, nullptr, ptrWheelShape, sLeftWheelData); + m_ptrRightWheel = std::make_shared(*this, 1, nullptr, ptrWheelShape, sRightWheelData); + /* copy the bodies to the base class */ + m_vecBodies = {m_ptrBody, m_ptrLeftWheel, m_ptrRightWheel}; + /* synchronize with the entity with the space */ + Reset(); + } + + /****************************************/ + /****************************************/ + + void CDynamics3DEPuckModel::Reset() { + /* reset the base class */ + CDynamics3DMultiBodyObjectModel::Reset(); + /* set up wheels */ + m_cMultiBody.setupRevolute(m_ptrLeftWheel->GetIndex(), + m_ptrLeftWheel->GetData().Mass, + m_ptrLeftWheel->GetData().Inertia, + m_ptrBody->GetIndex(), + m_cBodyToLeftWheelJointRotation, + btVector3(0.0, 1.0, 0.0), + m_cBodyToLeftWheelJointOffset, + m_cLeftWheelToBodyJointOffset, + true); + m_cMultiBody.setupRevolute(m_ptrRightWheel->GetIndex(), + m_ptrRightWheel->GetData().Mass, + m_ptrRightWheel->GetData().Inertia, + m_ptrBody->GetIndex(), + m_cBodyToRightWheelJointRotation, + btVector3(0.0, 1.0, 0.0), + m_cBodyToRightWheelJointOffset, + m_cRightWheelToBodyJointOffset, + true); + /* set up motors for the wheels */ + m_ptrLeftMotor = + std::make_unique(&m_cMultiBody, + m_ptrLeftWheel->GetIndex(), + 0.0, + m_fWheelMotorMaxImpulse); + m_ptrRightMotor = + std::make_unique(&m_cMultiBody, + m_ptrRightWheel->GetIndex(), + 0.0, + m_fWheelMotorMaxImpulse); + /* Allocate memory and prepare the btMultiBody */ + m_cMultiBody.finalizeMultiDof(); + /* Synchronize with the entity in the space */ + UpdateEntityStatus(); + } + + /****************************************/ + /****************************************/ + + void CDynamics3DEPuckModel::CalculateBoundingBox() { + btVector3 cModelAabbMin, cModelAabbMax; + /* Initialize the bounding box with the base's AABB */ + m_ptrBody->GetShape().getAabb(m_ptrBody->GetTransform(), cModelAabbMin, cModelAabbMax); + /* Write back the bounding box swapping the coordinate systems and the Y component */ + GetBoundingBox().MinCorner.Set(cModelAabbMin.getX(), -cModelAabbMax.getZ(), cModelAabbMin.getY()); + GetBoundingBox().MaxCorner.Set(cModelAabbMax.getX(), -cModelAabbMin.getZ(), cModelAabbMax.getY()); + } + + /****************************************/ + /****************************************/ + + void CDynamics3DEPuckModel::UpdateEntityStatus() { + /* run the base class's implementation of this method */ + CDynamics3DMultiBodyObjectModel::UpdateEntityStatus(); + } + + /****************************************/ + /****************************************/ + + void CDynamics3DEPuckModel::UpdateFromEntityStatus() { + /* run the base class's implementation of this method */ + CDynamics3DMultiBodyObjectModel::UpdateFromEntityStatus(); + /* update joint velocities */ + m_ptrLeftMotor->setVelocityTarget(m_cWheeledEntity.GetWheelVelocities()[0]); + m_ptrRightMotor->setVelocityTarget(m_cWheeledEntity.GetWheelVelocities()[1]); + } + + /****************************************/ + /****************************************/ + + void CDynamics3DEPuckModel::AddToWorld(btMultiBodyDynamicsWorld& c_world) { + /* run the base class's implementation of this method */ + CDynamics3DMultiBodyObjectModel::AddToWorld(c_world); + /* add the actuators (btMultiBodyJointMotors) constraints to the world */ + c_world.addMultiBodyConstraint(m_ptrLeftMotor.get()); + c_world.addMultiBodyConstraint(m_ptrRightMotor.get()); + } + + /****************************************/ + /****************************************/ + + void CDynamics3DEPuckModel::RemoveFromWorld(btMultiBodyDynamicsWorld& c_world) { + /* remove the actuators (btMultiBodyJointMotors) constraints from the world */ + c_world.removeMultiBodyConstraint(m_ptrRightMotor.get()); + c_world.removeMultiBodyConstraint(m_ptrLeftMotor.get()); + /* run the base class's implementation of this method */ + CDynamics3DMultiBodyObjectModel::RemoveFromWorld(c_world); + } + + /****************************************/ + /****************************************/ + + REGISTER_STANDARD_DYNAMICS3D_OPERATIONS_ON_ENTITY(CEPuckEntity, CDynamics3DEPuckModel); + + /****************************************/ + /****************************************/ + + const btVector3 CDynamics3DEPuckModel::m_cBodyHalfExtents(0.0362, 0.0236, 0.0362); + const btScalar CDynamics3DEPuckModel::m_fBodyMass(0.242); + const btTransform CDynamics3DEPuckModel::m_cBodyOffset(btQuaternion(0.0, 0.0, 0.0, 1.0), btVector3(0.0,0.00125,0.0)); + const btTransform CDynamics3DEPuckModel::m_cBodyGeometricOffset(btQuaternion(0.0, 0.0, 0.0, 1.0), btVector3(0.0, -0.0236, 0.0)); + const btVector3 CDynamics3DEPuckModel::m_cWheelHalfExtents(0.02125,0.0015,0.02125); + const btScalar CDynamics3DEPuckModel::m_fWheelMass(0.006); + const btTransform CDynamics3DEPuckModel::m_cWheelGeometricOffset(btQuaternion(0.0, 0.0, 0.0, 1.0), btVector3(0.0,-0.0015,0.0)); + const btTransform CDynamics3DEPuckModel::m_cLeftWheelOffset(btQuaternion(btVector3(-1,0,0), SIMD_HALF_PI), btVector3(0.0, 0.02125, -0.0255)); + const btTransform CDynamics3DEPuckModel::m_cRightWheelOffset(btQuaternion(btVector3(1,0,0), SIMD_HALF_PI), btVector3(0.0, 0.02125, 0.0255)); + const btVector3 CDynamics3DEPuckModel::m_cBodyToRightWheelJointOffset(0.0, -0.0036, 0.0255); + const btVector3 CDynamics3DEPuckModel::m_cRightWheelToBodyJointOffset(0.0, 0.0015, 0.0); + const btQuaternion CDynamics3DEPuckModel::m_cBodyToRightWheelJointRotation(btVector3(-1,0,0), SIMD_HALF_PI); + const btVector3 CDynamics3DEPuckModel::m_cBodyToLeftWheelJointOffset(0.0, -0.0036, -0.0255); + const btVector3 CDynamics3DEPuckModel::m_cLeftWheelToBodyJointOffset(0.0, 0.0015, -0.0); + const btQuaternion CDynamics3DEPuckModel::m_cBodyToLeftWheelJointRotation(btVector3(1,0,0), SIMD_HALF_PI); + /* TODO calibrate these values */ + const btScalar CDynamics3DEPuckModel::m_fWheelMotorMaxImpulse(0.15); + const btScalar CDynamics3DEPuckModel::m_fWheelFriction(5.0); + + /****************************************/ + /****************************************/ + +} diff --git a/src/plugins/robots/e-puck/simulator/dynamics3d_epuck_model.h b/src/plugins/robots/e-puck/simulator/dynamics3d_epuck_model.h new file mode 100644 index 00000000..d70faa7c --- /dev/null +++ b/src/plugins/robots/e-puck/simulator/dynamics3d_epuck_model.h @@ -0,0 +1,75 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#ifndef DYNAMICS3D_EPUCK_MODEL_H +#define DYNAMICS3D_EPUCK_MODEL_H + +namespace argos { + class CDynamics3DEPuckModel; + class CEPuckEntity; + class CWheeledEntity; +} + +#include +#include +#include + +namespace argos { + + class CDynamics3DEPuckModel : public CDynamics3DMultiBodyObjectModel { + + public: + + CDynamics3DEPuckModel(CDynamics3DEngine& c_engine, + CEPuckEntity& c_epuck); + + virtual ~CDynamics3DEPuckModel() {} + + virtual void Reset(); + + virtual void CalculateBoundingBox(); + + virtual void UpdateEntityStatus(); + + virtual void UpdateFromEntityStatus(); + + virtual void AddToWorld(btMultiBodyDynamicsWorld& c_world); + + virtual void RemoveFromWorld(btMultiBodyDynamicsWorld& c_world); + + private: + /* joint constraints */ + std::unique_ptr m_ptrLeftMotor; + std::unique_ptr m_ptrRightMotor; + /* links */ + std::shared_ptr m_ptrBody; + std::shared_ptr m_ptrLeftWheel; + std::shared_ptr m_ptrRightWheel; + /* entities */ + CWheeledEntity& m_cWheeledEntity; + /* lower base data */ + static const btVector3 m_cBodyHalfExtents; + static const btScalar m_fBodyMass; + static const btTransform m_cBodyOffset; + static const btTransform m_cBodyGeometricOffset; + /* wheel data */ + static const btVector3 m_cWheelHalfExtents; + static const btScalar m_fWheelMass; + static const btTransform m_cWheelGeometricOffset; + static const btTransform m_cLeftWheelOffset; + static const btTransform m_cRightWheelOffset; + static const btVector3 m_cBodyToRightWheelJointOffset; + static const btVector3 m_cRightWheelToBodyJointOffset; + static const btQuaternion m_cBodyToRightWheelJointRotation; + static const btVector3 m_cBodyToLeftWheelJointOffset; + static const btVector3 m_cLeftWheelToBodyJointOffset; + static const btQuaternion m_cBodyToLeftWheelJointRotation; + static const btScalar m_fWheelMotorMaxImpulse; + static const btScalar m_fWheelFriction; + }; +} + +#endif From b2e97d2da3b4233464484e5a2e184d40b167a20a Mon Sep 17 00:00:00 2001 From: Michael Allwright Date: Fri, 8 Nov 2019 16:37:35 +0100 Subject: [PATCH 4/4] Fix bug in the positioning sensor control interface (was not compatible with CLuaQuaternion) --- .../ci_positioning_sensor.cpp | 22 +++---------------- 1 file changed, 3 insertions(+), 19 deletions(-) diff --git a/src/plugins/robots/generic/control_interface/ci_positioning_sensor.cpp b/src/plugins/robots/generic/control_interface/ci_positioning_sensor.cpp index c3c718a5..0da2219d 100644 --- a/src/plugins/robots/generic/control_interface/ci_positioning_sensor.cpp +++ b/src/plugins/robots/generic/control_interface/ci_positioning_sensor.cpp @@ -37,25 +37,9 @@ namespace argos { #ifdef ARGOS_WITH_LUA void CCI_PositioningSensor::ReadingsToLuaState(lua_State* pt_lua_state) { lua_getfield(pt_lua_state, -1, "positioning"); - lua_getfield(pt_lua_state, -1, "position"); - lua_pushnumber(pt_lua_state, m_sReading.Position.GetX()); - lua_setfield(pt_lua_state, -2, "x"); - lua_pushnumber(pt_lua_state, m_sReading.Position.GetY()); - lua_setfield(pt_lua_state, -2, "y"); - lua_pushnumber(pt_lua_state, m_sReading.Position.GetZ()); - lua_setfield(pt_lua_state, -2, "z"); - m_sReading.Orientation.ToAngleAxis(m_cAngle, m_cAxis); - lua_getfield (pt_lua_state, -2, "orientation"); - lua_pushnumber(pt_lua_state, m_cAngle.GetValue()); - lua_setfield (pt_lua_state, -2, "angle"); - lua_getfield (pt_lua_state, -1, "axis"); - lua_pushnumber(pt_lua_state, m_cAxis.GetX()); - lua_setfield (pt_lua_state, -2, "x"); - lua_pushnumber(pt_lua_state, m_cAxis.GetY()); - lua_setfield (pt_lua_state, -2, "y"); - lua_pushnumber(pt_lua_state, m_cAxis.GetZ()); - lua_setfield (pt_lua_state, -2, "z"); - lua_pop(pt_lua_state, 4); + CLuaUtility::AddToTable(pt_lua_state, "position", m_sReading.Position); + CLuaUtility::AddToTable(pt_lua_state, "orientation", m_sReading.Orientation); + lua_pop(pt_lua_state, 1); } #endif