diff --git a/src/plugins/robots/CMakeLists.txt b/src/plugins/robots/CMakeLists.txt index 2f58b230..b3806975 100644 --- a/src/plugins/robots/CMakeLists.txt +++ b/src/plugins/robots/CMakeLists.txt @@ -19,3 +19,7 @@ endif(ARGOS_BUILD_FOR_SIMULATOR OR ARGOS_BUILD_FOR STREQUAL "e-puck") if(ARGOS_BUILD_FOR_SIMULATOR OR ARGOS_BUILD_FOR STREQUAL "spiri") add_subdirectory(spiri) endif(ARGOS_BUILD_FOR_SIMULATOR OR ARGOS_BUILD_FOR STREQUAL "spiri") + +if(ARGOS_BUILD_FOR_SIMULATOR) + add_subdirectory(prototype) +endif(ARGOS_BUILD_FOR_SIMULATOR) diff --git a/src/plugins/robots/prototype/CMakeLists.txt b/src/plugins/robots/prototype/CMakeLists.txt new file mode 100644 index 00000000..7d1266d1 --- /dev/null +++ b/src/plugins/robots/prototype/CMakeLists.txt @@ -0,0 +1,90 @@ +# +# Prototype headers +# +# argos3/plugins/robots/prototype/control_interface +set(ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_CONTROLINTERFACE + control_interface/ci_prototype_joints_sensor.h + control_interface/ci_prototype_joints_actuator.h) +# argos3/plugins/robots/prototype/simulator +if(ARGOS_BUILD_FOR_SIMULATOR) + set(ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_SIMULATOR + simulator/prototype_joints_default_actuator.h + simulator/prototype_entity.h + simulator/prototype_joint_entity.h + simulator/prototype_joint_equipped_entity.h + simulator/prototype_link_entity.h + simulator/prototype_link_equipped_entity.h + simulator/dynamics3d_prototype_model.h + simulator/prototype_joints_default_sensor.h) +endif(ARGOS_BUILD_FOR_SIMULATOR) + +# +# Prototype sources +# +# argos3/plugins/robots/prototype/control_interface +set(ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE + ${ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_CONTROLINTERFACE} + control_interface/ci_prototype_joints_actuator.cpp + control_interface/ci_prototype_joints_sensor.cpp) +# argos3/plugins/robots/prototype/simulator +if(ARGOS_BUILD_FOR_SIMULATOR) + set(ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE + ${ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE} + ${ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_SIMULATOR} + simulator/dynamics3d_prototype_model.cpp + simulator/prototype_entity.cpp + simulator/prototype_joint_entity.cpp + simulator/prototype_joint_equipped_entity.cpp + simulator/prototype_joints_default_actuator.cpp + simulator/prototype_joints_default_sensor.cpp + simulator/prototype_link_entity.cpp + simulator/prototype_link_equipped_entity.cpp) + # Compile the graphical visualization only if the necessary libraries have been found + include(ARGoSCheckQTOpenGL) + if(ARGOS_COMPILE_QTOPENGL) + set(ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_SIMULATOR + ${ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_SIMULATOR} + simulator/qtopengl_prototype.h) + set(ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE + ${ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE} + simulator/qtopengl_prototype.cpp) + endif(ARGOS_COMPILE_QTOPENGL) +endif(ARGOS_BUILD_FOR_SIMULATOR) + +# +# Create the prototype plugin +# + +include_directories( + ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet + ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/BroadphaseCollision + ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/CollisionDispatch + ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/CollisionShapes + ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/Gimpact + ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/NarrowPhaseCollision + ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletDynamics/ConstraintSolver + ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletDynamics/Dynamics + ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletDynamics/Vehicle + ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletDynamics/Character + ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/LinearMath) + +add_library(argos3plugin_${ARGOS_BUILD_FOR}_prototype SHARED ${ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE}) +target_link_libraries(argos3plugin_${ARGOS_BUILD_FOR}_prototype + argos3plugin_${ARGOS_BUILD_FOR}_genericrobot + argos3plugin_${ARGOS_BUILD_FOR}_dynamics3d) +if(ARGOS_COMPILE_QTOPENGL) + target_link_libraries(argos3plugin_${ARGOS_BUILD_FOR}_prototype argos3plugin_${ARGOS_BUILD_FOR}_qtopengl) +endif(ARGOS_COMPILE_QTOPENGL) + +install(FILES ${ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_CONTROLINTERFACE} + DESTINATION include/argos3/plugins/robots/prototype/control_interface) + +if(ARGOS_BUILD_FOR_SIMULATOR) + install(FILES ${ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_SIMULATOR} + DESTINATION include/argos3/plugins/robots/prototype/simulator) +endif(ARGOS_BUILD_FOR_SIMULATOR) + +install(TARGETS argos3plugin_${ARGOS_BUILD_FOR}_prototype + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib/argos3 + ARCHIVE DESTINATION lib/argos3) diff --git a/src/plugins/robots/prototype/control_interface/ci_prototype_joints_actuator.cpp b/src/plugins/robots/prototype/control_interface/ci_prototype_joints_actuator.cpp new file mode 100644 index 00000000..4cddf1d0 --- /dev/null +++ b/src/plugins/robots/prototype/control_interface/ci_prototype_joints_actuator.cpp @@ -0,0 +1,55 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#include "ci_prototype_joints_actuator.h" + +#ifdef ARGOS_WITH_LUA +#include +#endif + +namespace argos { + + /****************************************/ + /****************************************/ + +#ifdef ARGOS_WITH_LUA + int LuaSetTarget(lua_State* pt_lua_state) { + /* get a reference to the actuator */ + CCI_PrototypeJointsActuator::SActuator* pc_actuator = + reinterpret_cast(lua_touserdata(pt_lua_state, lua_upvalueindex(1))); + /* check number of arguments */ + if(lua_gettop(pt_lua_state) != 1) { + std::string strErrMsg = "robot.joints." + pc_actuator->Id + ".set_target() requires 1 argument"; + return luaL_error(pt_lua_state, strErrMsg.c_str()); + } + luaL_checktype(pt_lua_state, 1, LUA_TNUMBER); + /* Perform action */ + pc_actuator->Target = lua_tonumber(pt_lua_state, 1); + return 0; + } +#endif + + /****************************************/ + /****************************************/ + +#ifdef ARGOS_WITH_LUA + void CCI_PrototypeJointsActuator::CreateLuaState(lua_State* pt_lua_state) { + CLuaUtility::StartTable(pt_lua_state, "joints"); + for(SActuator* pc_actuator : m_vecActuators) { + CLuaUtility::StartTable(pt_lua_state, pc_actuator->Id); + /* push a pointer to each actuator onto the lua stack */ + lua_pushstring(pt_lua_state, "set_target"); + lua_pushlightuserdata(pt_lua_state, pc_actuator); + lua_pushcclosure(pt_lua_state, &LuaSetTarget, 1); + lua_settable(pt_lua_state, -3); + CLuaUtility::EndTable(pt_lua_state); + } + CLuaUtility::EndTable(pt_lua_state); + } +#endif + + +} diff --git a/src/plugins/robots/prototype/control_interface/ci_prototype_joints_actuator.h b/src/plugins/robots/prototype/control_interface/ci_prototype_joints_actuator.h new file mode 100644 index 00000000..6edc85ec --- /dev/null +++ b/src/plugins/robots/prototype/control_interface/ci_prototype_joints_actuator.h @@ -0,0 +1,46 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#ifndef CCI_PROTOTYPE_JOINTS_ACTUATOR_H +#define CCI_PROTOTYPE_JOINTS_ACTUATOR_H + +namespace argos { + class CCI_PrototypeJointsActuator; +} + +#include +#include + +namespace argos { + + class CCI_PrototypeJointsActuator : virtual public CCI_Actuator { + + public: + struct SActuator { + using TVector = std::vector; + SActuator(const std::string& str_id, Real f_target) : + Id(str_id), + Target(f_target) {} + std::string Id; + Real Target; + }; + + public: + /** + * Destructor. + */ + virtual ~CCI_PrototypeJointsActuator() {} + +#ifdef ARGOS_WITH_LUA + virtual void CreateLuaState(lua_State*); +#endif + + protected: + SActuator::TVector m_vecActuators; + }; +} + +#endif diff --git a/src/plugins/robots/prototype/control_interface/ci_prototype_joints_sensor.cpp b/src/plugins/robots/prototype/control_interface/ci_prototype_joints_sensor.cpp new file mode 100644 index 00000000..42b03b94 --- /dev/null +++ b/src/plugins/robots/prototype/control_interface/ci_prototype_joints_sensor.cpp @@ -0,0 +1,51 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#include "ci_prototype_joints_sensor.h" + +#ifdef ARGOS_WITH_LUA +#include +#endif + +namespace argos { + + /****************************************/ + /****************************************/ + +#ifdef ARGOS_WITH_LUA + void CCI_PrototypeJointsSensor::CreateLuaState(lua_State* pt_lua_state) { + /* CLuaUtility::OpenRobotStateTable is required since we don't know + whether the joint actuator has been added to the robot nor do we + know which joints it may have added to the robot's table in Lua */ + CLuaUtility::OpenRobotStateTable(pt_lua_state, "joints"); + for(SSensor* pc_sensor : m_vecSensors) { + CLuaUtility::OpenRobotStateTable(pt_lua_state, pc_sensor->Id); + CLuaUtility::AddToTable(pt_lua_state, "encoder", pc_sensor->Value); + CLuaUtility::CloseRobotStateTable(pt_lua_state); + } + CLuaUtility::CloseRobotStateTable(pt_lua_state); + } +#endif + + /****************************************/ + /****************************************/ + +#ifdef ARGOS_WITH_LUA + void CCI_PrototypeJointsSensor::ReadingsToLuaState(lua_State* pt_lua_state) { + lua_getfield(pt_lua_state, -1, "joints"); + for(SSensor* pc_sensor : m_vecSensors) { + lua_getfield(pt_lua_state, -1, (pc_sensor->Id).c_str()); + CLuaUtility::AddToTable(pt_lua_state, "encoder", pc_sensor->Value); + lua_pop(pt_lua_state, 1); + } + lua_pop(pt_lua_state, 1); + } +#endif + + /****************************************/ + /****************************************/ + +} diff --git a/src/plugins/robots/prototype/control_interface/ci_prototype_joints_sensor.h b/src/plugins/robots/prototype/control_interface/ci_prototype_joints_sensor.h new file mode 100644 index 00000000..548fa55f --- /dev/null +++ b/src/plugins/robots/prototype/control_interface/ci_prototype_joints_sensor.h @@ -0,0 +1,50 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#ifndef CCI_PROTOTYPE_JOINTS_SENSOR_H +#define CCI_PROTOTYPE_JOINTS_SENSOR_H + +namespace argos { + class CCI_PrototypeJointsSensor; +} + +#include +#include +#include + +namespace argos { + + class CCI_PrototypeJointsSensor : virtual public CCI_Sensor { + + public: + + struct SSensor { + using TVector = std::vector; + SSensor(const std::string& str_id) : + Id(str_id) {} + std::string Id; + Real Value; + }; + + public: + + /** + * Destructor. + */ + virtual ~CCI_PrototypeJointsSensor() {} + +#ifdef ARGOS_WITH_LUA + virtual void CreateLuaState(lua_State* pt_lua_state); + + void ReadingsToLuaState(lua_State* pt_lua_state); +#endif + + protected: + SSensor::TVector m_vecSensors; + }; +} + +#endif diff --git a/src/plugins/robots/prototype/simulator/dynamics3d_prototype_model.cpp b/src/plugins/robots/prototype/simulator/dynamics3d_prototype_model.cpp new file mode 100644 index 00000000..7d72a0cf --- /dev/null +++ b/src/plugins/robots/prototype/simulator/dynamics3d_prototype_model.cpp @@ -0,0 +1,474 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#include "dynamics3d_prototype_model.h" +#include +#include +#include +#include +#include +#include + +namespace argos { + + /****************************************/ + /****************************************/ + + btCollisionShape& CDynamics3DPrototypeModel::RequestShape(const CPrototypeLinkEntity& c_link_entity) { + btVector3 cHalfExtents(c_link_entity.GetExtents().GetX() * 0.5f, + c_link_entity.GetExtents().GetZ() * 0.5f, + c_link_entity.GetExtents().GetY() * 0.5f); + btCollisionShape* pcShape = nullptr; + switch(c_link_entity.GetGeometry()) { + case CPrototypeLinkEntity::EGeometry::BOX: + pcShape = CDynamics3DShapeManager::RequestBox(cHalfExtents); + break; + case CPrototypeLinkEntity::EGeometry::CYLINDER: + pcShape = CDynamics3DShapeManager::RequestCylinder(cHalfExtents); + break; + case CPrototypeLinkEntity::EGeometry::SPHERE: + pcShape = CDynamics3DShapeManager::RequestSphere(cHalfExtents.getZ()); + break; + default: + THROW_ARGOSEXCEPTION("Collision shape geometry not implemented"); + break; + } + return *pcShape; + } + + /****************************************/ + /****************************************/ + + CDynamics3DMultiBodyObjectModel::CAbstractBody::SData + CDynamics3DPrototypeModel::CreateBodyData(const CPrototypeLinkEntity& c_link_entity) { + /* retreive a collision shape */ + btCollisionShape& cCollisionShape = RequestShape(c_link_entity); + /* get friction */ + btScalar fFriction = GetEngine().GetDefaultFriction(); + /* calculate inertia */ + btScalar fMass = c_link_entity.GetMass(); + btVector3 cInertia; + cCollisionShape.calculateLocalInertia(fMass, cInertia); + /* calculate start transform */ + const CVector3& cPosition = c_link_entity.GetAnchor().Position; + const CQuaternion& cOrientation = c_link_entity.GetAnchor().Orientation; + btTransform cStartTransform(btQuaternion(cOrientation.GetX(), + cOrientation.GetZ(), + -cOrientation.GetY(), + cOrientation.GetW()), + btVector3(cPosition.GetX(), + cPosition.GetZ(), + -cPosition.GetY())); + /* calculate center of mass offset */ + btTransform cCenterOfMassOffset(btQuaternion(0.0f, 0.0f, 0.0f, 1.0f), + btVector3(0.0f, -c_link_entity.GetExtents().GetZ() * 0.5f, 0.0f)); + /* return link data */ + return CAbstractBody::SData(cStartTransform, cCenterOfMassOffset, cInertia, fMass, fFriction, cCollisionShape); + } + + /****************************************/ + /****************************************/ + + CDynamics3DPrototypeModel::CDynamics3DPrototypeModel(CDynamics3DEngine& c_engine, + CPrototypeEntity& c_entity) : + CDynamics3DMultiBodyObjectModel(c_engine, + c_entity, + c_entity.GetLinkEquippedEntity().GetLinks().size() - 1, + !c_entity.GetEmbodiedEntity().IsMovable()), + m_cEntity(c_entity), + m_cJointEquippedEntity(c_entity.GetJointEquippedEntity()) { + /* use the reference link as the base of the robot */ + CPrototypeLinkEntity& cBaseLink = c_entity.GetLinkEquippedEntity().GetReferenceLink(); + /* create the data for the link */ + const CAbstractBody::SData& sBaseLinkData = + CreateBodyData(cBaseLink); + /* set up the base link body */ + CBase* pcBase = new CBase(*this, cBaseLink.GetAnchor(), sBaseLinkData); + /* add to collection */ + m_vecBodies.push_back(pcBase); + /* counter for enumerating the links inside the btMultiBody */ + UInt32 unLinkIndex = 0; + /* copy all joints pointers into a temporary vector */ + CPrototypeJointEntity::TVector vecJointsToAdd = + m_cJointEquippedEntity.GetJoints(); + /* while there are joints to be added */ + while(! vecJointsToAdd.empty()) { + size_t unRemainingJoints = vecJointsToAdd.size(); + for(CPrototypeJointEntity::TVectorIterator it_joint = std::begin(vecJointsToAdd); + it_joint != std::end(vecJointsToAdd); + ++it_joint) { + /* get a reference to the joint */ + CPrototypeJointEntity& cJoint = (**it_joint); + /* get a reference to the parent link entity */ + const CPrototypeLinkEntity& cParentLink = cJoint.GetParentLink(); + /* check if the parent link has been added */ + CAbstractBody::TVectorIterator itParentLinkBody = + std::find_if(std::begin(m_vecBodies), + std::end(m_vecBodies), + [&cParentLink] (CAbstractBody* pc_body) { + return (cParentLink.GetAnchor().Index == pc_body->GetAnchor().Index); + }); + /* if the parent link hasn't been added yet, try the next joint */ + if(itParentLinkBody == std::end(m_vecBodies)) { + continue; + } + /* there should be no need to check the result of this cast */ + CLink* pcParentLinkBody = dynamic_cast(*itParentLinkBody); + /* setup child link */ + const CAbstractBody::SData& sChildLinkData = + CreateBodyData(cJoint.GetChildLink()); + /* set up the child link body */ + CLink* pcChildLinkBody = new CLink(*this, + unLinkIndex++, + cJoint.GetChildLink().GetAnchor(), + sChildLinkData); + /* add to collection */ + m_vecBodies.push_back(pcChildLinkBody); + /* calculate joint parameters for parent link */ + const CVector3& cParentOffsetPosition = cJoint.GetParentLinkJointPosition(); + const CQuaternion& cParentOffsetOrientation = cJoint.GetParentLinkJointOrientation(); + btTransform cParentOffsetTransform = + btTransform(btQuaternion(cParentOffsetOrientation.GetX(), + cParentOffsetOrientation.GetZ(), + -cParentOffsetOrientation.GetY(), + cParentOffsetOrientation.GetW()), + btVector3(cParentOffsetPosition.GetX(), + cParentOffsetPosition.GetZ(), + -cParentOffsetPosition.GetY())); + cParentOffsetTransform = pcParentLinkBody->GetData().CenterOfMassOffset * cParentOffsetTransform; + /* calculate joint parameters for child link */ + const CVector3& cChildOffsetPosition = cJoint.GetChildLinkJointPosition(); + const CQuaternion& cChildOffsetOrientation = cJoint.GetChildLinkJointOrientation(); + btTransform cChildOffsetTransform = + btTransform(btQuaternion(cChildOffsetOrientation.GetX(), + cChildOffsetOrientation.GetZ(), + -cChildOffsetOrientation.GetY(), + cChildOffsetOrientation.GetW()), + btVector3(cChildOffsetPosition.GetX(), + cChildOffsetPosition.GetZ(), + -cChildOffsetPosition.GetY())); + cChildOffsetTransform = pcChildLinkBody->GetData().CenterOfMassOffset * cChildOffsetTransform; + /* calculate the joint axis */ + btVector3 cJointAxis(cJoint.GetJointAxis().GetX(), + cJoint.GetJointAxis().GetZ(), + -cJoint.GetJointAxis().GetY()); + /* calculate the parent to child joint rotation */ + /* + // From testing, these both the of following solutions seem correct although one may be wrong + btQuaternion cParentToChildRotation = cParentOffsetTransform.inverse().getRotation() * + cChildOffsetTransform.getRotation(); + */ + btQuaternion cParentToChildRotation = cChildOffsetTransform.getRotation() * + cParentOffsetTransform.inverse().getRotation(); + /* store joint configuration for reset */ + m_vecJoints.emplace_back(cJoint.GetType(), + *pcParentLinkBody, + *pcChildLinkBody, + cParentOffsetTransform.getOrigin(), + -cChildOffsetTransform.getOrigin(), + cParentToChildRotation, + cJointAxis, + cJoint.GetDisableCollision()); + /* sensor and actuator configuration for revolute and prismatic joints */ + if(cJoint.GetType() == CPrototypeJointEntity::EType::REVOLUTE || + cJoint.GetType() == CPrototypeJointEntity::EType::PRISMATIC) { + /* if the joint actuator isn't disabled */ + CPrototypeJointEntity::SActuator& sActuator = cJoint.GetActuator(); + if(sActuator.Mode != CPrototypeJointEntity::SActuator::EMode::DISABLED) { + m_vecActuators.emplace_back(*this, sActuator, pcChildLinkBody->GetIndex()); + } + /* and if the joint sensor isn't disabled */ + CPrototypeJointEntity::SSensor& sSensor = cJoint.GetSensor(); + if(sSensor.Mode != CPrototypeJointEntity::SSensor::EMode::DISABLED) { + m_vecSensors.emplace_back(*this, sSensor, pcChildLinkBody->GetIndex()); + } + } + /* joint limits */ + if(cJoint.HasLimit()) { + if(cJoint.GetType() == CPrototypeJointEntity::EType::REVOLUTE) { + const CRange& cLimit = cJoint.GetLimit().Revolute; + m_vecLimits.emplace_back(*this, + cLimit.GetMin().GetValue(), + cLimit.GetMax().GetValue(), + pcChildLinkBody->GetIndex()); + } + else if(cJoint.GetType() == CPrototypeJointEntity::EType::PRISMATIC) { + const CRange& cLimit = cJoint.GetLimit().Prismatic; + m_vecLimits.emplace_back(*this, + cLimit.GetMin(), + cLimit.GetMax(), + pcChildLinkBody->GetIndex()); + } + } + /* now that the joint and link has been added we remove it from the vector + and restart the loop */ + vecJointsToAdd.erase(it_joint); + break; + } + if(unRemainingJoints == vecJointsToAdd.size()) { + CPrototypeJointEntity* pcJoint = vecJointsToAdd.front(); + THROW_ARGOSEXCEPTION("Can not add link \"" << + pcJoint->GetChildLink().GetId() << + "\" to the model before its parent link \"" << + pcJoint->GetParentLink().GetId() << + "\" has been added."); + } + } + for(CPrototypeLinkEntity* pc_link : m_cEntity.GetLinkEquippedEntity().GetLinks()) { + if(std::find_if(std::begin(m_vecBodies), + std::end(m_vecBodies), + [pc_link] (CAbstractBody* pc_body) { + return (pc_link->GetAnchor().Index == pc_body->GetAnchor().Index); + }) == std::end(m_vecBodies)) { + THROW_ARGOSEXCEPTION("Link \"" << pc_link->GetId() << + "\" is not connected to the model."); + + } + } + /* with this call to reset, the multi-body model is configured and ready to be added */ + Reset(); + } + + /****************************************/ + /****************************************/ + + void CDynamics3DPrototypeModel::AddToWorld(btMultiBodyDynamicsWorld& c_world) { + /* call CDynamics3DMultiBodyObjectModel::AddToWorld to add all bodies to the world */ + CDynamics3DMultiBodyObjectModel::AddToWorld(c_world); + /* add the actuators (btMultiBodyJointMotors) constraints to the world */ + for(SActuator& s_actuator : m_vecActuators) { + c_world.addMultiBodyConstraint(&s_actuator.Motor); + } + /* add the joint limits to the world */ + for(SLimit& s_limit : m_vecLimits) { + c_world.addMultiBodyConstraint(&s_limit.Constraint); + } + } + + /****************************************/ + /****************************************/ + + void CDynamics3DPrototypeModel::RemoveFromWorld(btMultiBodyDynamicsWorld& c_world) { + /* remove the joint limits to the world */ + for(SLimit& s_limit : m_vecLimits) { + c_world.removeMultiBodyConstraint(&s_limit.Constraint); + } + /* remove the actuators (btMultiBodyJointMotors) constraints to the world */ + for(SActuator& s_actuator : m_vecActuators) { + c_world.removeMultiBodyConstraint(&s_actuator.Motor); + } + /* call CDynamics3DMultiBodyObjectModel::RemoveFromWorld to remove all bodies to the world */ + CDynamics3DMultiBodyObjectModel::RemoveFromWorld(c_world); + } + + /****************************************/ + /****************************************/ + + void CDynamics3DPrototypeModel::Reset() { + /* reset the base class (recreates the btMultiBody and calls reset on the bodies) */ + CDynamics3DMultiBodyObjectModel::Reset(); + /* add setup the links and joints */ + for(SJoint& s_joint : m_vecJoints) { + switch(s_joint.Type) { + case CPrototypeJointEntity::EType::FIXED: + m_cMultiBody.setupFixed(s_joint.Child.GetIndex(), + s_joint.Child.GetData().Mass, + s_joint.Child.GetData().Inertia, + s_joint.Parent.GetIndex(), + s_joint.ParentToChildRotation, + s_joint.ParentOffset, + s_joint.ChildOffset); + break; + case CPrototypeJointEntity::EType::SPHERICAL: + m_cMultiBody.setupSpherical(s_joint.Child.GetIndex(), + s_joint.Child.GetData().Mass, + s_joint.Child.GetData().Inertia, + s_joint.Parent.GetIndex(), + s_joint.ParentToChildRotation, + s_joint.ParentOffset, + s_joint.ChildOffset, + s_joint.DisableCollision); + break; + case CPrototypeJointEntity::EType::REVOLUTE: + m_cMultiBody.setupRevolute(s_joint.Child.GetIndex(), + s_joint.Child.GetData().Mass, + s_joint.Child.GetData().Inertia, + s_joint.Parent.GetIndex(), + s_joint.ParentToChildRotation, + s_joint.Axis, + s_joint.ParentOffset, + s_joint.ChildOffset, + s_joint.DisableCollision); + break; + case CPrototypeJointEntity::EType::PRISMATIC: + m_cMultiBody.setupPrismatic(s_joint.Child.GetIndex(), + s_joint.Child.GetData().Mass, + s_joint.Child.GetData().Inertia, + s_joint.Parent.GetIndex(), + s_joint.ParentToChildRotation, + s_joint.Axis, + s_joint.ParentOffset, + s_joint.ChildOffset, + s_joint.DisableCollision); + break; + default: + break; + } + } + /* reset the actuators */ + for(SActuator& s_actuator : m_vecActuators) { + s_actuator.Reset(); + } + /* reset the joint limits */ + for(SLimit& s_limit : m_vecLimits) { + s_limit.Reset(); + } + /* Allocate memory and prepare the btMultiBody */ + m_cMultiBody.finalizeMultiDof(); + /* Synchronize with the entity in the space */ + UpdateEntityStatus(); + } + + /****************************************/ + /****************************************/ + + void CDynamics3DPrototypeModel::UpdateEntityStatus() { + /* write back sensor data to the joints */ + for(SSensor& s_sensor : m_vecSensors) { + s_sensor.Update(); + } + /* update anchors using the base class method */ + CDynamics3DMultiBodyObjectModel::UpdateEntityStatus(); + } + + /****************************************/ + /****************************************/ + + void CDynamics3DPrototypeModel::UpdateFromEntityStatus() { + /* read in actuator data from the joints */ + for(SActuator& s_actuator : m_vecActuators) { + s_actuator.Update(); + } + } + + /****************************************/ + /****************************************/ + + CDynamics3DPrototypeModel::SJoint::SJoint(CPrototypeJointEntity::EType e_type, + CLink& c_parent, + CLink& c_child, + const btVector3& c_parent_offset, + const btVector3& c_child_offset, + const btQuaternion& c_parent_to_child_rotation, + const btVector3& c_axis, + bool b_disable_collision) : + Type(e_type), + Parent(c_parent), + Child(c_child), + ParentOffset(c_parent_offset), + ChildOffset(c_child_offset), + ParentToChildRotation(c_parent_to_child_rotation), + Axis(c_axis), + DisableCollision(b_disable_collision) {} + + /****************************************/ + /****************************************/ + + CDynamics3DPrototypeModel::SLimit::SLimit(CDynamics3DPrototypeModel& c_model, + Real f_lower_limit, + Real f_upper_limit, + SInt32 n_joint_index) : + Model(c_model), + LowerLimit(f_lower_limit), + UpperLimit(f_upper_limit), + JointIndex(n_joint_index), + Constraint(&c_model.GetMultiBody(), + n_joint_index, + f_lower_limit, + f_upper_limit) {} + + /****************************************/ + /****************************************/ + + void CDynamics3DPrototypeModel::SLimit::Reset() { + Constraint.~btMultiBodyJointLimitConstraint(); + new (&Constraint) btMultiBodyJointLimitConstraint(&Model.GetMultiBody(), + JointIndex, + LowerLimit, + UpperLimit); + } + + /****************************************/ + /****************************************/ + + CDynamics3DPrototypeModel::SActuator::SActuator(CDynamics3DPrototypeModel& c_model, + CPrototypeJointEntity::SActuator& s_actuator, + SInt32 n_joint_index) : + Model(c_model), + Actuator(s_actuator), + JointIndex(n_joint_index), + Motor(&c_model.GetMultiBody(), + n_joint_index, + 0, + s_actuator.Target, + s_actuator.MaxImpulse) {} + + /****************************************/ + /****************************************/ + + void CDynamics3DPrototypeModel::SActuator::Reset() { + Motor.~btMultiBodyJointMotor(); + new (&Motor) btMultiBodyJointMotor(&Model.GetMultiBody(), + JointIndex, + 0, + Actuator.Target, + Actuator.MaxImpulse); + } + + /****************************************/ + /****************************************/ + + void CDynamics3DPrototypeModel::SActuator::Update() { + if(Actuator.Mode == CPrototypeJointEntity::SActuator::EMode::POSITION) { + Motor.setPositionTarget(Actuator.Target); + } + else if(Actuator.Mode == CPrototypeJointEntity::SActuator::EMode::VELOCITY) { + Motor.setVelocityTarget(Actuator.Target); + } + } + + /****************************************/ + /****************************************/ + + CDynamics3DPrototypeModel::SSensor::SSensor(CDynamics3DPrototypeModel& c_model, + CPrototypeJointEntity::SSensor& s_sensor, + SInt32 n_joint_index) : + Model(c_model), + Sensor(s_sensor), + JointIndex(n_joint_index) {} + + /****************************************/ + /****************************************/ + + void CDynamics3DPrototypeModel::SSensor::Update() { + if(Sensor.Mode == CPrototypeJointEntity::SSensor::EMode::POSITION) { + Sensor.Value = Model.GetMultiBody().getJointPos(JointIndex); + } + else if(Sensor.Mode == CPrototypeJointEntity::SSensor::EMode::VELOCITY) { + Sensor.Value = Model.GetMultiBody().getJointVel(JointIndex); + } + } + + /****************************************/ + /****************************************/ + + REGISTER_STANDARD_DYNAMICS3D_OPERATIONS_ON_ENTITY(CPrototypeEntity, CDynamics3DPrototypeModel); + + /****************************************/ + /****************************************/ + +} + diff --git a/src/plugins/robots/prototype/simulator/dynamics3d_prototype_model.h b/src/plugins/robots/prototype/simulator/dynamics3d_prototype_model.h new file mode 100644 index 00000000..7077aeac --- /dev/null +++ b/src/plugins/robots/prototype/simulator/dynamics3d_prototype_model.h @@ -0,0 +1,123 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#ifndef DYNAMICS3D_PROTOTYPE_MODEL_H +#define DYNAMICS3D_PROTOTYPE_MODEL_H + +namespace argos { + class CPrototypeEntity; + class CPrototypeJointEquippedEntity; +} + +#include +#include +#include +#include + +namespace argos { + + class CDynamics3DPrototypeModel : public CDynamics3DMultiBodyObjectModel { + + public: + + CDynamics3DPrototypeModel(CDynamics3DEngine& c_engine, CPrototypeEntity& c_entity); + + virtual ~CDynamics3DPrototypeModel() {} + + virtual void Reset(); + + virtual void UpdateEntityStatus(); + + virtual void UpdateFromEntityStatus(); + + virtual void AddToWorld(btMultiBodyDynamicsWorld& c_world); + + virtual void RemoveFromWorld(btMultiBodyDynamicsWorld& c_world); + + private: + + CAbstractBody::SData CreateBodyData(const CPrototypeLinkEntity& c_link_entity); + + btCollisionShape& RequestShape(const CPrototypeLinkEntity& c_link_entity); + + private: + + CPrototypeEntity& m_cEntity; + + CPrototypeJointEquippedEntity& m_cJointEquippedEntity; + + struct SJoint { + SJoint(CPrototypeJointEntity::EType e_type, + CLink& c_parent, + CLink& c_child, + const btVector3& c_parent_offset, + const btVector3& c_child_offset, + const btQuaternion& c_parent_to_child_rotation, + const btVector3& c_axis, + bool b_disable_collision); + CPrototypeJointEntity::EType Type; + CLink& Parent; + CLink& Child; + btVector3 ParentOffset; + btVector3 ChildOffset; + btQuaternion ParentToChildRotation; + btVector3 Axis; + bool DisableCollision; + }; + + struct SLimit { + /* constructor */ + SLimit(CDynamics3DPrototypeModel& c_model, + Real f_lower_limit, + Real f_upper_limit, + SInt32 n_joint_index); + /* methods */ + void Reset(); + /* data members */ + CDynamics3DPrototypeModel& Model; + Real LowerLimit; + Real UpperLimit; + SInt32 JointIndex; + btMultiBodyJointLimitConstraint Constraint; + }; + + struct SActuator { + /* constructor */ + SActuator(CDynamics3DPrototypeModel& c_model, + CPrototypeJointEntity::SActuator& s_actuator, + SInt32 n_joint_index); + /* methods */ + void Reset(); + void Update(); + /* members */ + CDynamics3DPrototypeModel& Model; + CPrototypeJointEntity::SActuator& Actuator; + SInt32 JointIndex; + btMultiBodyJointMotor Motor; + }; + + struct SSensor { + /* constructor */ + SSensor(CDynamics3DPrototypeModel& c_model, + CPrototypeJointEntity::SSensor& s_sensor, + SInt32 n_joint_index); + /* methods */ + void Update(); + /* members */ + CDynamics3DPrototypeModel& Model; + CPrototypeJointEntity::SSensor& Sensor; + SInt32 JointIndex; + }; + + std::vector m_vecJoints; + std::vector m_vecLimits; + std::vector m_vecActuators; + std::vector m_vecSensors; + + }; +} + +#endif diff --git a/src/plugins/robots/prototype/simulator/prototype_entity.cpp b/src/plugins/robots/prototype/simulator/prototype_entity.cpp new file mode 100644 index 00000000..138d03f7 --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_entity.cpp @@ -0,0 +1,293 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#include "prototype_entity.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace argos { + + /****************************************/ + /****************************************/ + + CPrototypeEntity::CPrototypeEntity() : + CComposableEntity(nullptr), + m_pcEmbodiedEntity(nullptr), + m_pcControllableEntity(nullptr), + m_pcLinkEquippedEntity(nullptr), + m_pcJointEquippedEntity(nullptr), + m_pcDirectionalLEDEquippedEntity(nullptr), + m_pcLEDEquippedEntity(nullptr), + m_pcTagEquippedEntity(nullptr), + m_pcRadioEquippedEntity(nullptr) {} + + /****************************************/ + /****************************************/ + + void CPrototypeEntity::Init(TConfigurationNode& t_tree) { + try { + /* Initialize the parent */ + CComposableEntity::Init(t_tree); + /* Check if the model is movable */ + bool bMovable = true; + GetNodeAttributeOrDefault(t_tree, "movable", bMovable, bMovable); + /* Initialize the body */ + m_pcEmbodiedEntity = new CEmbodiedEntity(this); + AddComponent(*m_pcEmbodiedEntity); + m_pcEmbodiedEntity->Init(GetNode(t_tree, "body")); + m_pcEmbodiedEntity->SetMovable(bMovable); + /* Initialize the links */ + m_pcLinkEquippedEntity = new CPrototypeLinkEquippedEntity(this); + m_pcLinkEquippedEntity->Init(GetNode(t_tree, "links")); + AddComponent(*m_pcLinkEquippedEntity); + /* Initialize the joints */ + m_pcJointEquippedEntity = new CPrototypeJointEquippedEntity(this); + AddComponent(*m_pcJointEquippedEntity); + /* create an empty joints configuration node if one isn't provided */ + if(!NodeExists(t_tree, "joints")) { + TConfigurationNode tJointsNode("joints"); + AddChildNode(t_tree, tJointsNode); + } + m_pcJointEquippedEntity->Init(GetNode(t_tree, "joints")); + /* Initialize the devices */ + if(NodeExists(t_tree, "devices")) { + TConfigurationNodeIterator itDevice; + for(itDevice = itDevice.begin(&GetNode(t_tree, "devices")); + itDevice != itDevice.end(); + ++itDevice) { + if(itDevice->Value() == "directional_leds" ) { + m_pcDirectionalLEDEquippedEntity = + new CDirectionalLEDEquippedEntity(this); + m_pcDirectionalLEDEquippedEntity->Init(*itDevice); + AddComponent(*m_pcDirectionalLEDEquippedEntity); + /* Add the directional LEDs to the medium */ + std::string strMedium; + GetNodeAttribute(*itDevice, "medium", strMedium); + CDirectionalLEDMedium& cDirectionalLEDMedium = + CSimulator::GetInstance().GetMedium(strMedium); + m_pcDirectionalLEDEquippedEntity->SetMedium(cDirectionalLEDMedium); + m_pcDirectionalLEDEquippedEntity->Enable(); + } + else if(itDevice->Value() == "leds" ) { + m_pcLEDEquippedEntity = new CLEDEquippedEntity(this); + m_pcLEDEquippedEntity->Init(*itDevice); + AddComponent(*m_pcLEDEquippedEntity); + /* Add the LEDs to the medium */ + std::string strMedium; + GetNodeAttribute(*itDevice, "medium", strMedium); + CLEDMedium& cLEDMedium = + CSimulator::GetInstance().GetMedium(strMedium); + m_pcLEDEquippedEntity->SetMedium(cLEDMedium); + m_pcLEDEquippedEntity->Enable(); + } + else if(itDevice->Value() == "tags" ) { + m_pcTagEquippedEntity = new CTagEquippedEntity(this); + m_pcTagEquippedEntity->Init(*itDevice); + AddComponent(*m_pcTagEquippedEntity); + /* Add the tags to the medium */ + std::string strMedium; + GetNodeAttribute(*itDevice, "medium", strMedium); + CTagMedium& cTagMedium = + CSimulator::GetInstance().GetMedium(strMedium); + m_pcTagEquippedEntity->SetMedium(cTagMedium); + m_pcTagEquippedEntity->Enable(); + } + else if(itDevice->Value() == "radios" ) { + CRadioEquippedEntity* m_pcRadioEquippedEntity = + new CRadioEquippedEntity(this); + m_pcRadioEquippedEntity->Init(*itDevice); + AddComponent(*m_pcRadioEquippedEntity); + /* Add the radios to the medium */ + std::string strMedium; + GetNodeAttribute(*itDevice, "medium", strMedium); + CRadioMedium& cRadioMedium = + CSimulator::GetInstance().GetMedium(strMedium); + m_pcRadioEquippedEntity->SetMedium(cRadioMedium); + m_pcRadioEquippedEntity->Enable(); + } + else if(itDevice->Value() == "magnets" ) { + m_pcMagnetEquippedEntity = new CMagnetEquippedEntity(this); + m_pcMagnetEquippedEntity->Init(*itDevice); + AddComponent(*m_pcMagnetEquippedEntity); + } + else { + THROW_ARGOSEXCEPTION("Device type \"" << itDevice->Value() << "\" not implemented"); + } + } + } + /* Initialize the controllable entity, this is done last so that the sensors and + actuators correctly link to the entity */ + if(NodeExists(t_tree, "controller")) { + m_pcControllableEntity = new CControllableEntity(this); + AddComponent(*m_pcControllableEntity); + m_pcControllableEntity->Init(GetNode(t_tree, "controller")); + } + /* Update components */ + UpdateComponents(); + } + catch(CARGoSException& ex) { + THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex); + } + } + + /****************************************/ + /****************************************/ + + REGISTER_ENTITY(CPrototypeEntity, + "prototype", + "1.0", + "Michael Allwright [allsey87@gmail.com]", + "A generic and configurable entity", + "The prototyping entity allows users to quickly implement new entities using an\n" + "experiment's configuration file. A prototype entity is predominantly described\n" + "as a set of links connected through joints. The joints can be fixed, prismatic,\n" + "revolute, spherical, or planar. Sensors and actuators for prismatic and revolute\n" + "joints are provided. Since each link defines an anchor in the embodied entity,\n" + "it is also possible to attach the following entities to any link in the model:\n" + "the directional LED entity, the LED entity, the magnet entity, the tag entity,\n" + "the radio entity.\n\n" + "REQUIRED XML CONFIGURATION\n\n" + " \n" + " ...\n" + " \n" + " \n" + " \n" + " \n" + " \n" + " ...\n" + " \n\n" + "The 'id' attribute is necessary and must be unique among the entities. If two\n" + "entities share the same id the initialization aborts and the simulator will\n" + "terminate. The movable attribute defines whether the reference link of a robot\n" + "is fixed or not. This attribute is useful, for example, if you want to model a\n" + "robotic arm whose links are movable, but where there is one link (i.e. the\n" + "reference link) that is fixed to the ground, a wall, or the ceiling.\n" + "The body tag defines the position and orientation of the prototype entity in the\n" + "global coordinate system. The most simple entity contains a single link and no\n" + "joints. An important detail to note is the 'ref' attribute of the links node.\n" + "This attribute specifies which link is to be used to refer to the prototype\n" + "entity's location. The example in the required configuration defines a single\n" + "link with a box geometry. Three geometries for links are currently supported:\n" + "boxes, cylinders, and spheres. The size of a box is defined using the size\n" + "attribute while cylinders and spheres are defined using a radius attribute and a\n" + "height attribute in the case of the cylinder. The mass of each link must be\n" + "defined and specified in kilograms. The position and orientation of each link\n" + "refer to the offset of the link in the entity's local coordinate system.\n\n" + "OPTIONAL XML CONFIGURATION\n\n" + "In the following example, additional links and joints are added to a prototype\n" + "entity to form a small four-wheeled vehicle.\n\n" + " \n" + " ...\n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " ...\n" + " \n\n" + "In this example, there are five links: the base of the vehicle and four wheels.\n" + "The wheels are attached to the base using revolute joints. The joint node\n" + "requires the joint type attribute and in the case of prismatic and revolute\n" + "joints an axis which defines the joint's degree of freedom. In addition to the\n" + "axis attribute, prismatic and revolute joints can also be limited using the\n" + "limit attribute. In the case of prismatic joints, this value is interpreted as a\n" + "range in meters and in the case of revolute joints, this value is interpreted as\n" + "a range in degrees. The maximum and minimum values must be separated with the\n" + "colon character. The joint node requires two child nodes, namely the parent and\n" + "child nodes which describe the links involved in the joint. Each of these nodes\n" + "also specifies a position and an orientation, which defines the offset from the\n" + "link's local coordinate system to the joint's reference frame. Note that the\n" + "prototype entity is consistent with ARGoS conventions and the origin of all\n" + "coordinate systems is at the base of an entity and not at its center of mass.\n" + "Prismatic and revolute joints support sensors and actuators which can control\n" + "and monitor the position and velocity of a joint. Refer to the documentation on\n" + "the joints sensor and joints actuator by querying the joints plugin. It is also\n" + "possible to attach any number of devices to the links in a prototype entity:\n\n" + " \n" + " ...\n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " ...\n" + " \n\n" + "Each of these devices has three attributes in common: an anchor, a position, and\n" + "an orientation. Using these attributes you can attach any number of these\n" + "entities to any link in the prototype entity since each link creates an anchor.\n" + "In addition to these attributes, the entities have some attributes that define\n" + "their characteristics. For example, tags have a side length and payload, while\n" + "the directional LED has a color. The three devices in the example all require\n" + "mediums. These mediums index the entities and allow them to be detected by\n" + "sensors.", + "Usable" + ); + + /****************************************/ + /****************************************/ + + REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CPrototypeEntity); + + /****************************************/ + /****************************************/ +} diff --git a/src/plugins/robots/prototype/simulator/prototype_entity.h b/src/plugins/robots/prototype/simulator/prototype_entity.h new file mode 100644 index 00000000..9e5df2b5 --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_entity.h @@ -0,0 +1,145 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#ifndef PROTOTYPE_ENTITY_H +#define PROTOTYPE_ENTITY_H + +namespace argos { + class CEmbodiedEntity; + class CControllableEntity; + class CPrototypeLinkEquippedEntity; + class CPrototypeJointEquippedEntity; + class CTagEquippedEntity; + class CRadioEquippedEntity; + class CMagnetEquippedEntity; + class CCameraEquippedEntity; + class CDirectionalLEDEquippedEntity; + class CLEDEquippedEntity; + class CPrototypeEntity; +} + +#include + +namespace argos { + + class CPrototypeEntity : public CComposableEntity { + + public: + + ENABLE_VTABLE(); + + public: + + CPrototypeEntity(); + + virtual ~CPrototypeEntity() {} + + virtual void Init(TConfigurationNode& t_tree); + + inline CEmbodiedEntity& GetEmbodiedEntity() { + return *m_pcEmbodiedEntity; + } + + inline CPrototypeLinkEquippedEntity& GetLinkEquippedEntity() { + return *m_pcLinkEquippedEntity; + } + + inline CPrototypeJointEquippedEntity& GetJointEquippedEntity() { + return *m_pcJointEquippedEntity; + } + + inline bool HasControllableEntity() const { + return (m_pcControllableEntity != nullptr); + } + + inline CControllableEntity& GetControllableEntity() { + ARGOS_ASSERT(m_pcControllableEntity != nullptr, + "CPrototypeEntity::GetControllableEntity(), id=\"" << + GetId() + GetContext() << + "\": was not initialized with a controllable entity"); + return *m_pcControllableEntity; + } + + inline bool HasDirectionalLEDEquippedEntity() const { + return (m_pcDirectionalLEDEquippedEntity != nullptr); + } + + inline CDirectionalLEDEquippedEntity& GetDirectionalLEDEquippedEntity() { + ARGOS_ASSERT(m_pcDirectionalLEDEquippedEntity != nullptr, + "CPrototypeEntity::GetDirectionalLEDEquippedEntity(), id=\"" << + GetId() + GetContext() << + "\": was not initialized with an directional LED equipped entity"); + return *m_pcDirectionalLEDEquippedEntity; + } + + inline bool HasLEDEquippedEntity() const { + return (m_pcLEDEquippedEntity != nullptr); + } + + inline CLEDEquippedEntity& GetLEDEquippedEntity() { + ARGOS_ASSERT(m_pcLEDEquippedEntity != nullptr, + "CPrototypeEntity::GetLEDEquippedEntity(), id=\"" << + GetId() + GetContext() << + "\": was not initialized with an LED equipped entity"); + return *m_pcLEDEquippedEntity; + } + + inline bool HasTagEquippedEntity() const { + return (m_pcTagEquippedEntity != nullptr); + } + + inline CTagEquippedEntity& GetTagEquippedEntity() { + ARGOS_ASSERT(m_pcTagEquippedEntity != nullptr, + "CPrototypeEntity::GetTagEquippedEntity(), id=\"" << + GetId() + GetContext() << + "\": was not initialized with a tag equipped entity"); + return *m_pcTagEquippedEntity; + } + + inline bool HasRadioEquippedEntity() const { + return (m_pcRadioEquippedEntity != nullptr); + } + + inline CRadioEquippedEntity& GetRadioEquippedEntity() { + ARGOS_ASSERT(m_pcRadioEquippedEntity != nullptr, + "CPrototypeEntity::GetRadioEquippedEntity(), id=\"" << + GetId() + GetContext() << + "\": was not initialized with a radio equipped entity"); + return *m_pcRadioEquippedEntity; + } + + inline bool HasMagnetEquippedEntity() const { + return (m_pcMagnetEquippedEntity != nullptr); + } + + inline CMagnetEquippedEntity& GetMagnetEquippedEntity() { + ARGOS_ASSERT(m_pcMagnetEquippedEntity != nullptr, + "CPrototypeEntity::GetMagnetEquippedEntity(), id=\"" << + GetId() + GetContext() << + "\": was not initialized with a magnet equipped entity"); + return *m_pcMagnetEquippedEntity; + } + + virtual std::string GetTypeDescription() const { + return "prototype"; + } + + private: + + CEmbodiedEntity* m_pcEmbodiedEntity; + CControllableEntity* m_pcControllableEntity; + CPrototypeLinkEquippedEntity* m_pcLinkEquippedEntity; + CPrototypeJointEquippedEntity* m_pcJointEquippedEntity; + CDirectionalLEDEquippedEntity* m_pcDirectionalLEDEquippedEntity; + CLEDEquippedEntity* m_pcLEDEquippedEntity; + CTagEquippedEntity* m_pcTagEquippedEntity; + CRadioEquippedEntity* m_pcRadioEquippedEntity; + CMagnetEquippedEntity* m_pcMagnetEquippedEntity; + }; + +} + +#endif diff --git a/src/plugins/robots/prototype/simulator/prototype_joint_entity.cpp b/src/plugins/robots/prototype/simulator/prototype_joint_entity.cpp new file mode 100644 index 00000000..5be0cba6 --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_joint_entity.cpp @@ -0,0 +1,121 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#include "prototype_joint_entity.h" +#include + +namespace argos { + + /****************************************/ + /****************************************/ + + CPrototypeJointEntity::CPrototypeJointEntity(CComposableEntity* pc_parent) : + CComposableEntity(pc_parent), + m_pcParentLink(nullptr), + m_pcChildLink(nullptr), + m_bDisableCollision(true), + m_eType(EType::FIXED), + m_bHasLimit(false) {} + + /****************************************/ + /****************************************/ + + CPrototypeJointEntity::CPrototypeJointEntity(CComposableEntity* pc_parent, + const std::string& str_id) : + CComposableEntity(pc_parent, str_id), + m_pcParentLink(nullptr), + m_pcChildLink(nullptr), + m_bDisableCollision(true), + m_eType(EType::FIXED), + m_bHasLimit(false) {} + + /****************************************/ + /****************************************/ + + void CPrototypeJointEntity::Init(TConfigurationNode& t_tree) { + try { + /* Init parent */ + CComposableEntity::Init(t_tree); + /* Get parent and child links */ + CPrototypeLinkEquippedEntity& cLinkEquippedEntity = + GetParent().GetParent().GetComponent("links"); + /* Get the disable collisions with parent flag */ + GetNodeAttributeOrDefault(t_tree, + "disable_collision", + m_bDisableCollision, + m_bDisableCollision); + /* Get joint type */ + std::string strJointType; + GetNodeAttribute(t_tree, "type", strJointType); + if(strJointType == "fixed") { + m_eType = EType::FIXED; + } + else if(strJointType == "prismatic") { + m_eType = EType::PRISMATIC; + GetNodeAttribute(t_tree, "axis", m_cJointAxis); + if(NodeAttributeExists(t_tree, "limit")) { + m_bHasLimit = true; + GetNodeAttribute(t_tree, "limit", m_uLimit.Prismatic); + } + } + else if(strJointType == "revolute") { + m_eType = EType::REVOLUTE; + GetNodeAttribute(t_tree, "axis", m_cJointAxis); + if(NodeAttributeExists(t_tree, "limit")) { + m_bHasLimit = true; + CRange cJointLimit; + GetNodeAttribute(t_tree, "limit", cJointLimit); + m_uLimit.Revolute = CRange(ToRadians(cJointLimit.GetMin()), + ToRadians(cJointLimit.GetMax())); + } + } + else if(strJointType == "spherical") { + m_eType = EType::SPHERICAL; + } + else { + THROW_ARGOSEXCEPTION("Joint type \"" << strJointType << "\" is not implemented"); + } + /* parse the parent node */ + TConfigurationNode& tJointParentNode = GetNode(t_tree, "parent"); + std::string strJointParentLink; + GetNodeAttribute(tJointParentNode, "link", strJointParentLink); + m_pcParentLink = &(cLinkEquippedEntity.GetLink(strJointParentLink)); + GetNodeAttributeOrDefault(tJointParentNode, + "position", + m_cParentLinkJointPosition, + m_cParentLinkJointPosition); + GetNodeAttributeOrDefault(tJointParentNode, + "orientation", + m_cParentLinkJointOrientation, + m_cParentLinkJointOrientation); + /* parse the child node */ + TConfigurationNode& tJointChildNode = GetNode(t_tree, "child"); + std::string strJointChildLink; + GetNodeAttribute(tJointChildNode, "link", strJointChildLink); + m_pcChildLink = &(cLinkEquippedEntity.GetLink(strJointChildLink)); + GetNodeAttributeOrDefault(tJointChildNode, + "position", + m_cChildLinkJointPosition, + m_cChildLinkJointPosition); + GetNodeAttributeOrDefault(tJointChildNode, + "orientation", + m_cChildLinkJointOrientation, + m_cChildLinkJointOrientation); + } + catch(CARGoSException& ex) { + THROW_ARGOSEXCEPTION_NESTED("Error while initializing joint entity", ex); + } + } + + /****************************************/ + /****************************************/ + + REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CPrototypeJointEntity); + + /****************************************/ + /****************************************/ + +} diff --git a/src/plugins/robots/prototype/simulator/prototype_joint_entity.h b/src/plugins/robots/prototype/simulator/prototype_joint_entity.h new file mode 100644 index 00000000..c59255b3 --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_joint_entity.h @@ -0,0 +1,167 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#ifndef PROTOTYPE_JOINT_ENTITY_H +#define PROTOTYPE_JOINT_ENTITY_H + +namespace argos { + class CPrototypeLinkEntity; + class CPrototypeJointEntity; +} + +#include +#include +#include + +namespace argos { + + class CPrototypeJointEntity : public CComposableEntity { + + public: + + ENABLE_VTABLE(); + + using TVector = std::vector; + using TVectorIterator = std::vector::iterator; + using TVectorConstIterator = std::vector::const_iterator; + + public: + enum class EType { + FIXED, + PRISMATIC, + REVOLUTE, + SPHERICAL, + PLANAR, + }; + + union ULimit { + ULimit() {} + CRange Prismatic; + CRange Revolute; + }; + + struct SActuator { + enum class EMode { + DISABLED, + POSITION, + VELOCITY, + }; + Real Target = 0.0f; + Real MaxImpulse = 0.0f; + EMode Mode = EMode::DISABLED; + }; + + struct SSensor { + enum class EMode { + DISABLED, + POSITION, + VELOCITY, + }; + Real Value = 0.0f; + EMode Mode = EMode::DISABLED; + }; + + public: + + CPrototypeJointEntity(CComposableEntity* pc_parent); + + CPrototypeJointEntity(CComposableEntity* pc_parent, + const std::string& str_id); + + virtual ~CPrototypeJointEntity() {} + + virtual void Init(TConfigurationNode& t_tree); + + virtual std::string GetTypeDescription() const { + return "joint"; + } + + inline CPrototypeLinkEntity& GetParentLink() { + return *m_pcParentLink; + } + + inline const CPrototypeLinkEntity& GetParentLink() const { + return *m_pcParentLink; + } + + inline const CVector3& GetParentLinkJointPosition() const { + return m_cParentLinkJointPosition; + } + + inline const CQuaternion& GetParentLinkJointOrientation() const { + return m_cParentLinkJointOrientation; + } + + inline CPrototypeLinkEntity& GetChildLink() { + return *m_pcChildLink; + } + + inline const CPrototypeLinkEntity& GetChildLink() const { + return *m_pcChildLink; + } + + inline const CVector3& GetChildLinkJointPosition() const { + return m_cChildLinkJointPosition; + } + + inline const CQuaternion& GetChildLinkJointOrientation() const { + return m_cChildLinkJointOrientation; + } + + inline bool GetDisableCollision() const { + return m_bDisableCollision; + } + + inline EType GetType() const { + return m_eType; + } + + inline const CVector3& GetJointAxis() const { + return m_cJointAxis; + } + + bool HasLimit() { + return m_bHasLimit; + } + + const ULimit& GetLimit() const { + ARGOS_ASSERT(m_bHasLimit == true, + "CPrototypeJointEntity::GetLimit(), id=\"" << + GetId() << + "\": joint was not initialized with a limit"); + return m_uLimit; + } + + SSensor& GetSensor() { + return m_cSensor; + } + + SActuator& GetActuator() { + return m_cActuator; + } + + private: + + CPrototypeLinkEntity* m_pcParentLink; + CVector3 m_cParentLinkJointPosition; + CQuaternion m_cParentLinkJointOrientation; + + CPrototypeLinkEntity* m_pcChildLink; + CVector3 m_cChildLinkJointPosition; + CQuaternion m_cChildLinkJointOrientation; + + bool m_bDisableCollision; + EType m_eType; + CVector3 m_cJointAxis; + bool m_bHasLimit; + ULimit m_uLimit; + SSensor m_cSensor; + SActuator m_cActuator; + }; + +} + +#endif diff --git a/src/plugins/robots/prototype/simulator/prototype_joint_equipped_entity.cpp b/src/plugins/robots/prototype/simulator/prototype_joint_equipped_entity.cpp new file mode 100644 index 00000000..99f293bc --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_joint_equipped_entity.cpp @@ -0,0 +1,88 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#include "prototype_joint_equipped_entity.h" + +namespace argos { + + /****************************************/ + /****************************************/ + + CPrototypeJointEquippedEntity::CPrototypeJointEquippedEntity(CComposableEntity* pc_parent) : + CComposableEntity(pc_parent) {} + + /****************************************/ + /****************************************/ + + CPrototypeJointEquippedEntity::CPrototypeJointEquippedEntity(CComposableEntity* pc_parent, + const std::string& str_id) : + CComposableEntity(pc_parent, str_id) {} + + /****************************************/ + /****************************************/ + + void CPrototypeJointEquippedEntity::Init(TConfigurationNode& t_tree) { + try { + /* Init parent */ + CComposableEntity::Init(t_tree); + /* Go through the joints */ + TConfigurationNodeIterator itJoint("joint"); + for(itJoint = itJoint.begin(&t_tree); + itJoint != itJoint.end(); + ++itJoint) { + /* for each node */ + CPrototypeJointEntity* pcJointEntity = new CPrototypeJointEntity(this); + pcJointEntity->Init(*itJoint); + AddComponent(*pcJointEntity); + m_vecJoints.push_back(pcJointEntity); + } + } + catch(CARGoSException& ex) { + THROW_ARGOSEXCEPTION_NESTED("Failed to initialize joint equipped entity \"" << GetId() << "\".", ex); + } + } + + /****************************************/ + /****************************************/ + + CPrototypeJointEntity& CPrototypeJointEquippedEntity::GetJoint(const std::string& str_id) { + CPrototypeJointEntity::TVectorIterator itJoint = + std::find_if(std::begin(m_vecJoints), + std::end(m_vecJoints), + [str_id] (const CPrototypeJointEntity* pc_joint) { + return (pc_joint->GetId() == str_id); + }); + ARGOS_ASSERT(itJoint != std::end(m_vecJoints), + "CPrototypeJointEquippedEntity::GetJoint(), id=\"" << + GetId() << + "\": joint not found: str_id = " << + str_id); + return **itJoint; + } + + /****************************************/ + /****************************************/ + + CPrototypeJointEntity& CPrototypeJointEquippedEntity::GetJoint(UInt32 un_index) { + ARGOS_ASSERT(un_index < m_vecJoints.size(), + "CPrototypeJointEquippedEntity::GetJoint(), id=\"" << + GetId() << + "\": index out of bounds: un_index = " << + un_index << + ", m_vecJoints.size() = " << + m_vecJoints.size()); + return *m_vecJoints[un_index]; + } + + /****************************************/ + /****************************************/ + + REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CPrototypeJointEquippedEntity); + + /****************************************/ + /****************************************/ + +} diff --git a/src/plugins/robots/prototype/simulator/prototype_joint_equipped_entity.h b/src/plugins/robots/prototype/simulator/prototype_joint_equipped_entity.h new file mode 100644 index 00000000..02a875ae --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_joint_equipped_entity.h @@ -0,0 +1,57 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#ifndef PROTOTYPE_JOINT_EQUIPPED_ENTITY_H +#define PROTOTYPE_JOINT_EQUIPPED_ENTITY_H + +namespace argos { + class CPrototypeJointEquippedEntity; + class CPrototypeJointEntity; +} + +#include +#include +#include + +namespace argos { + + class CPrototypeJointEquippedEntity : public CComposableEntity { + + public: + + ENABLE_VTABLE(); + + public: + + CPrototypeJointEquippedEntity(CComposableEntity* pc_parent); + + CPrototypeJointEquippedEntity(CComposableEntity* pc_parent, + const std::string& str_id); + + virtual ~CPrototypeJointEquippedEntity() {} + + virtual void Init(TConfigurationNode& t_tree); + + CPrototypeJointEntity& GetJoint(UInt32 un_index); + + CPrototypeJointEntity& GetJoint(const std::string& str_id); + + inline CPrototypeJointEntity::TVector& GetJoints() { + return m_vecJoints; + } + + virtual std::string GetTypeDescription() const { + return "joints"; + } + + private: + + CPrototypeJointEntity::TVector m_vecJoints; + }; + +} + +#endif diff --git a/src/plugins/robots/prototype/simulator/prototype_joints_default_actuator.cpp b/src/plugins/robots/prototype/simulator/prototype_joints_default_actuator.cpp new file mode 100644 index 00000000..b9923391 --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_joints_default_actuator.cpp @@ -0,0 +1,138 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#include "prototype_joints_default_actuator.h" + +namespace argos { + + /****************************************/ + /****************************************/ + + CPrototypeJointsDefaultActuator::CPrototypeJointsDefaultActuator() : + m_pcJointEquippedEntity(NULL) { + } + + /****************************************/ + /****************************************/ + + void CPrototypeJointsDefaultActuator::SetRobot(CComposableEntity& c_entity) { + m_pcJointEquippedEntity = &(c_entity.GetComponent("joints")); + } + + /****************************************/ + /****************************************/ + + void CPrototypeJointsDefaultActuator::Init(TConfigurationNode& t_tree) { + TConfigurationNodeIterator itJoint("joint"); + for(itJoint = itJoint.begin(&t_tree); + itJoint != itJoint.end(); + ++itJoint) { + /* parse the joint id */ + std::string strJointId; + GetNodeAttribute(*itJoint, "id", strJointId); + /* parse the maximum impulse */ + Real fMaxImpulse; + GetNodeAttribute(*itJoint, "max_impulse", fMaxImpulse); + /* get joint */ + CPrototypeJointEntity& cJoint = m_pcJointEquippedEntity->GetJoint(strJointId); + if(cJoint.GetType() == CPrototypeJointEntity::EType::PRISMATIC || + cJoint.GetType() == CPrototypeJointEntity::EType::REVOLUTE) { + /* get actuator */ + CPrototypeJointEntity::SActuator& sInstance = cJoint.GetActuator(); + /* configure actuator */ + std::string strMode; + GetNodeAttribute(*itJoint, "mode", strMode); + if(strMode == "disabled") { + sInstance.Mode = CPrototypeJointEntity::SActuator::EMode::DISABLED; + } + else if(strMode == "position") { + sInstance.Mode = CPrototypeJointEntity::SActuator::EMode::POSITION; + } + else if(strMode == "velocity") { + sInstance.Mode = CPrototypeJointEntity::SActuator::EMode::VELOCITY; + } + else { + THROW_ARGOSEXCEPTION("Actuator mode \"" << strMode << + "\" for joint \"" << strJointId << + "\" " << " is not implemented"); + } + sInstance.MaxImpulse = fMaxImpulse; + sInstance.Target = 0.0f; + /* create the actuator's interface */ + m_vecSimulatedActuators.emplace_back(strJointId, 0.0, sInstance); + } + else { + THROW_ARGOSEXCEPTION("Actuators can only be set on prismatic or revolute joints"); + } + } + for(SSimulatedActuator& s_actuator : m_vecSimulatedActuators) { + /* add joint actuators to the base class */ + m_vecActuators.push_back(&s_actuator); + } + } + + /****************************************/ + /****************************************/ + + void CPrototypeJointsDefaultActuator::Update() { + for(SSimulatedActuator& s_actuator : m_vecSimulatedActuators) { + s_actuator.Instance.Target = s_actuator.Target; + } + } + + /****************************************/ + /****************************************/ + + void CPrototypeJointsDefaultActuator::Reset() { + for(SSimulatedActuator& s_actuator : m_vecSimulatedActuators) { + s_actuator.Target = 0.0; + } + } + + /****************************************/ + /****************************************/ + + REGISTER_ACTUATOR(CPrototypeJointsDefaultActuator, + "joints", "default", + "Michael Allwright [allsey87@gmail.com]", + "1.0", + "The prototype joints actuator: controls a prototype entity's joints.", + "This actuator is used to control the joints inside a prototype entity. To\n" + "control a joint, add a joint child node to the joints node. Each child node has\n" + "three attributes, all of which are required.\n\n" + "REQUIRED XML CONFIGURATION\n\n" + " \n" + " ...\n" + " \n" + " ...\n" + " \n" + " ...\n" + " \n" + " \n" + " \n" + " ...\n" + " \n" + " ...\n" + " \n" + " ...\n" + " \n" + " ...\n" + " \n\n" + "The 'id' attribute specifies which joint in the prototype joint equipped entity\n" + "we are interested in controlling. The 'mode' attribute has three options:\n" + "disabled, position, and velocity. The disabled mode is self-explanatory. The\n" + "position and velocity modes enable closed loop position control and closed loop\n" + "velocity control. The 'max_impulse' attribute defines the maximum impulse of\n" + "the actuator in kg m/s for prismatic joints and kg m^2/s for revolute joints.\n\n" + "OPTIONAL XML CONFIGURATION\n\n" + "None.", + "Usable" + ); + + /****************************************/ + /****************************************/ + +} diff --git a/src/plugins/robots/prototype/simulator/prototype_joints_default_actuator.h b/src/plugins/robots/prototype/simulator/prototype_joints_default_actuator.h new file mode 100644 index 00000000..78ff2c8f --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_joints_default_actuator.h @@ -0,0 +1,58 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#ifndef PROTOTYPE_JOINTS_DEFAULT_ACTUATOR_H +#define PROTOTYPE_JOINTS_DEFAULT_ACTUATOR_H + +namespace argos { + class CPrototypeJointsDefaultActuator; +} + +#include +#include +#include +#include + +namespace argos { + + class CPrototypeJointsDefaultActuator : public CSimulatedActuator, + public CCI_PrototypeJointsActuator { + + public: + + struct SSimulatedActuator : SActuator { + SSimulatedActuator(const std::string& str_id, + Real f_target, + CPrototypeJointEntity::SActuator& s_actuator) : + SActuator(str_id, f_target), + Instance(s_actuator) {} + CPrototypeJointEntity::SActuator& Instance; + }; + + public: + + CPrototypeJointsDefaultActuator(); + + virtual ~CPrototypeJointsDefaultActuator() {} + + virtual void SetRobot(CComposableEntity& c_entity); + + virtual void Init(TConfigurationNode& t_tree); + + virtual void Update(); + + virtual void Reset(); + + private: + + std::vector m_vecSimulatedActuators; + + CPrototypeJointEquippedEntity* m_pcJointEquippedEntity; + + }; +} + +#endif diff --git a/src/plugins/robots/prototype/simulator/prototype_joints_default_sensor.cpp b/src/plugins/robots/prototype/simulator/prototype_joints_default_sensor.cpp new file mode 100644 index 00000000..f0434104 --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_joints_default_sensor.cpp @@ -0,0 +1,127 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#include "prototype_joints_default_sensor.h" + +namespace argos { + + /****************************************/ + /****************************************/ + + CPrototypeJointsDefaultSensor::CPrototypeJointsDefaultSensor() : + m_pcJointEquippedEntity(NULL) { + } + + /****************************************/ + /****************************************/ + + void CPrototypeJointsDefaultSensor::SetRobot(CComposableEntity& c_entity) { + m_pcJointEquippedEntity = &(c_entity.GetComponent("joints")); + } + + /****************************************/ + /****************************************/ + + void CPrototypeJointsDefaultSensor::Init(TConfigurationNode& t_tree) { + TConfigurationNodeIterator itJoint("joint"); + for(itJoint = itJoint.begin(&t_tree); + itJoint != itJoint.end(); + ++itJoint) { + /* parse the joint id */ + std::string strJointId; + GetNodeAttribute(*itJoint, "id", strJointId); + /* get joint */ + CPrototypeJointEntity& cJoint = m_pcJointEquippedEntity->GetJoint(strJointId); + /* get sensor */ + CPrototypeJointEntity::SSensor& sInstance = cJoint.GetSensor(); + /* configure sensor */ + std::string strMode; + GetNodeAttribute(*itJoint, "mode", strMode); + if(strMode == "disabled") { + sInstance.Mode = CPrototypeJointEntity::SSensor::EMode::DISABLED; + } + else if(strMode == "position") { + sInstance.Mode = CPrototypeJointEntity::SSensor::EMode::POSITION; + } + else if(strMode == "velocity") { + sInstance.Mode = CPrototypeJointEntity::SSensor::EMode::VELOCITY; + } + else { + THROW_ARGOSEXCEPTION("specified sensor mode for " << strJointId << " is not implemented"); + } + /* create the sensor's interface */ + m_vecSimulatedSensors.emplace_back(strJointId, sInstance); + } + for(SSimulatedSensor& s_sensor : m_vecSimulatedSensors) { + /* add joint actuators to the base class */ + m_vecSensors.push_back(&s_sensor); + } + } + + /****************************************/ + /****************************************/ + + void CPrototypeJointsDefaultSensor::Update() { + for(SSimulatedSensor& s_sensor : m_vecSimulatedSensors) { + s_sensor.Value = s_sensor.Instance.Value; + } + } + + /****************************************/ + /****************************************/ + + void CPrototypeJointsDefaultSensor::Reset() { + for(SSimulatedSensor& s_sensor : m_vecSimulatedSensors) { + s_sensor.Value = 0; + } + } + + /****************************************/ + /****************************************/ + + REGISTER_SENSOR(CPrototypeJointsDefaultSensor, + "joints", "default", + "Michael Allwright [allsey87@gmail.com]", + "1.0", + "The prototype joints sensor: monitors a prototype entity's joints.", + "This sensor is used to monitor the joints inside a prototype entity. To monitor\n" + "a joint, add a joint child node to the joints node. Each child node has two\n" + "required attributes.\n\n" + "REQUIRED XML CONFIGURATION\n\n" + " \n" + " ...\n" + " \n" + " ...\n" + " \n" + " ...\n" + " \n" + " \n" + " \n" + " ...\n" + " \n" + " ...\n" + " \n" + " ...\n" + " \n" + " ...\n" + " \n\n" + "The 'id' attribute specifies which joint in the prototype joint equipped entity\n" + "we are interested in monitoring. The 'mode' attribute has three options:\n" + "disabled, position, and velocity. The disabled mode is self-explanatory. The\n" + "position mode measures the displacement of the joint from its initial position\n" + "or orientation (depending on whether the specified joint is prismatic or\n" + "revolute). The reading from the joint is reported in either meters or radians\n" + "respectively. The velocity mode measures the how much the position or\n" + "orientation of the joint is changing every second.\n\n" + "OPTIONAL XML CONFIGURATION\n\n" + "None.", + "Usable" + ); + + /****************************************/ + /****************************************/ + +} diff --git a/src/plugins/robots/prototype/simulator/prototype_joints_default_sensor.h b/src/plugins/robots/prototype/simulator/prototype_joints_default_sensor.h new file mode 100644 index 00000000..8a7bf99a --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_joints_default_sensor.h @@ -0,0 +1,55 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#ifndef PROTOTYPE_JOINTS_DEFAULT_SENSOR_H +#define PROTOTYPE_JOINTS_DEFAULT_SENSOR_H + +namespace argos { + class CPrototypeJointsDefaultSensor; +} + +#include +#include +#include +#include + +namespace argos { + + class CPrototypeJointsDefaultSensor : public CSimulatedSensor, + public CCI_PrototypeJointsSensor { + + public: + struct SSimulatedSensor : public SSensor { + SSimulatedSensor(const std::string& str_id, + CPrototypeJointEntity::SSensor& s_sensor) : + SSensor(str_id), + Instance(s_sensor) {} + CPrototypeJointEntity::SSensor& Instance; + }; + + public: + CPrototypeJointsDefaultSensor(); + + virtual ~CPrototypeJointsDefaultSensor() {} + + virtual void SetRobot(CComposableEntity& c_entity); + + virtual void Init(TConfigurationNode& t_tree); + + virtual void Update(); + + virtual void Reset(); + + private: + + std::vector m_vecSimulatedSensors; + + CPrototypeJointEquippedEntity* m_pcJointEquippedEntity; + + }; +} + +#endif diff --git a/src/plugins/robots/prototype/simulator/prototype_link_entity.cpp b/src/plugins/robots/prototype/simulator/prototype_link_entity.cpp new file mode 100644 index 00000000..53abfc1f --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_link_entity.cpp @@ -0,0 +1,76 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#include "prototype_link_entity.h" +#include +#include + +namespace argos { + + /****************************************/ + /****************************************/ + + CPrototypeLinkEntity::CPrototypeLinkEntity(CComposableEntity* pc_parent) : + CEntity(pc_parent), + m_fMass(0.0f) {} + + /****************************************/ + /****************************************/ + + void CPrototypeLinkEntity::Init(TConfigurationNode& t_tree) { + try { + /* Init parent */ + CEntity::Init(t_tree); + /* Parse link geometry and dimensions */ + std::string strLinkGeometry; + GetNodeAttribute(t_tree, "geometry", strLinkGeometry); + if(strLinkGeometry == "box") { + m_eGeometry = BOX; + GetNodeAttribute(t_tree, "size", m_cExtents); + } else if(strLinkGeometry == "cylinder") { + m_eGeometry = CYLINDER; + Real fRadius; + Real fHeight; + GetNodeAttribute(t_tree, "height", fHeight); + GetNodeAttribute(t_tree, "radius", fRadius); + m_cExtents.Set(fRadius * 2.0f, fRadius * 2.0f, fHeight); + } else if(strLinkGeometry == "sphere") { + m_eGeometry = SPHERE; + Real fRadius; + GetNodeAttribute(t_tree, "radius", fRadius); + m_cExtents.Set(fRadius * 2.0f, fRadius * 2.0f, fRadius * 2.0f); + } else { + /* unknown geometry requested */ + THROW_ARGOSEXCEPTION("Geometry \"" << strLinkGeometry << "\" is not implemented"); + } + /* Parse link geometry and dimensions */ + GetNodeAttribute(t_tree, "mass", m_fMass); + /* Get the offset position of the link */ + CVector3 cOffsetPosition; + GetNodeAttributeOrDefault(t_tree, "position", cOffsetPosition, cOffsetPosition); + /* Get the offset orientation of the link */ + CQuaternion cOffsetOrientation; + GetNodeAttributeOrDefault(t_tree, "orientation", cOffsetOrientation, cOffsetOrientation); + /* Create an anchor for this link */ + CEmbodiedEntity& cBody = GetParent().GetParent().GetComponent("body"); + m_psAnchor = &(cBody.AddAnchor(GetId(), cOffsetPosition, cOffsetOrientation)); + /* Enable the anchor */ + m_psAnchor->Enable(); + } + catch(CARGoSException& ex) { + THROW_ARGOSEXCEPTION_NESTED("Error while initializing link entity", ex); + } + } + + /****************************************/ + /****************************************/ + + REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CPrototypeLinkEntity); + + /****************************************/ + /****************************************/ + +} diff --git a/src/plugins/robots/prototype/simulator/prototype_link_entity.h b/src/plugins/robots/prototype/simulator/prototype_link_entity.h new file mode 100644 index 00000000..fd353a66 --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_link_entity.h @@ -0,0 +1,80 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#ifndef PROTOTYPE_LINK_ENTITY_H +#define PROTOTYPE_LINK_ENTITY_H + +namespace argos { + class CPrototypeLinkEntity; +} + +#include +#include +#include +#include + +namespace argos { + + class CPrototypeLinkEntity : public CEntity { + + public: + + ENABLE_VTABLE(); + + using TVector = std::vector; + using TVectorIterator = std::vector::iterator; + using TVectorConstIterator = std::vector::const_iterator; + + public: + enum EGeometry { + CYLINDER, + BOX, + SPHERE, + }; + + public: + + CPrototypeLinkEntity(CComposableEntity* pc_parent); + + virtual ~CPrototypeLinkEntity() {} + + virtual void Init(TConfigurationNode& t_tree); + + virtual std::string GetTypeDescription() const { + return "link"; + } + + EGeometry GetGeometry() const { + return m_eGeometry; + } + + const CVector3& GetExtents() const { + return m_cExtents; + } + + Real GetMass() const { + return m_fMass; + } + + SAnchor& GetAnchor() { + return *m_psAnchor; + } + + const SAnchor& GetAnchor() const { + return *m_psAnchor; + } + + private: + + EGeometry m_eGeometry; + CVector3 m_cExtents; + Real m_fMass; + SAnchor* m_psAnchor; + }; + +} + +#endif diff --git a/src/plugins/robots/prototype/simulator/prototype_link_equipped_entity.cpp b/src/plugins/robots/prototype/simulator/prototype_link_equipped_entity.cpp new file mode 100644 index 00000000..6ff145ea --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_link_equipped_entity.cpp @@ -0,0 +1,107 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#include "prototype_link_equipped_entity.h" + +namespace argos { + + /****************************************/ + /****************************************/ + + CPrototypeLinkEquippedEntity::CPrototypeLinkEquippedEntity(CComposableEntity* pc_parent) : + CComposableEntity(pc_parent) {} + + /****************************************/ + /****************************************/ + + CPrototypeLinkEquippedEntity::CPrototypeLinkEquippedEntity(CComposableEntity* pc_parent, + const std::string& str_id) : + CComposableEntity(pc_parent, str_id) {} + + /****************************************/ + /****************************************/ + + void CPrototypeLinkEquippedEntity::Init(TConfigurationNode& t_tree) { + try { + /* Initialize the parent */ + CComposableEntity::Init(t_tree); + /* Initialize the links */ + TConfigurationNodeIterator itLink("link"); + for(itLink = itLink.begin(&t_tree); + itLink != itLink.end(); + ++itLink) { + CPrototypeLinkEntity* pcLinkEntity = new CPrototypeLinkEntity(this); + pcLinkEntity->Init(*itLink); + AddComponent(*pcLinkEntity); + m_vecLinks.push_back(pcLinkEntity); + } + /* Get the reference link */ + std::string strReferenceLink; + GetNodeAttribute(t_tree, "ref", strReferenceLink); + CPrototypeLinkEntity::TVectorIterator itReferenceLink = + std::find_if(std::begin(m_vecLinks), + std::end(m_vecLinks), + [strReferenceLink] (const CPrototypeLinkEntity* pc_link) { + return (pc_link->GetId() == strReferenceLink); + }); + /* Move the reference link to the front of the vector */ + if(itReferenceLink == std::end(m_vecLinks)) { + THROW_ARGOSEXCEPTION("The reference link \"" << strReferenceLink << "\" not found."); + } + else { + if(itReferenceLink != std::begin(m_vecLinks)) { + CPrototypeLinkEntity* pcReferenceLink = *itReferenceLink; + *itReferenceLink = m_vecLinks.front(); + m_vecLinks.front() = pcReferenceLink; + } + } + } + catch(CARGoSException& ex) { + THROW_ARGOSEXCEPTION_NESTED("Failed to initialize link equipped entity \"" << GetId() << "\".", ex); + } + } + + /****************************************/ + /****************************************/ + + CPrototypeLinkEntity& CPrototypeLinkEquippedEntity::GetLink(const std::string& str_id) { + CPrototypeLinkEntity::TVectorIterator itLink = + std::find_if(std::begin(m_vecLinks), + std::end(m_vecLinks), + [str_id] (const CPrototypeLinkEntity* pc_link) { + return (pc_link->GetId() == str_id); + }); + ARGOS_ASSERT(itLink != std::end(m_vecLinks), + "CPrototypeLinkEquippedEntity::GetLink(), id=\"" << + GetId() << + "\": link not found: str_id = " << + str_id); + return **itLink; + } + + /****************************************/ + /****************************************/ + + CPrototypeLinkEntity& CPrototypeLinkEquippedEntity::GetLink(UInt32 un_index) { + ARGOS_ASSERT(un_index < m_vecLinks.size(), + "CPrototypeLinkEquippedEntity::GetLink(), id=\"" << + GetId() << + "\": index out of bounds: un_index = " << + un_index << + ", m_vecLinks.size() = " << + m_vecLinks.size()); + return *m_vecLinks[un_index]; + } + + /****************************************/ + /****************************************/ + + REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CPrototypeLinkEquippedEntity); + + /****************************************/ + /****************************************/ + +} diff --git a/src/plugins/robots/prototype/simulator/prototype_link_equipped_entity.h b/src/plugins/robots/prototype/simulator/prototype_link_equipped_entity.h new file mode 100644 index 00000000..7dba282f --- /dev/null +++ b/src/plugins/robots/prototype/simulator/prototype_link_equipped_entity.h @@ -0,0 +1,61 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#ifndef PROTOTYPE_LINK_EQUIPPED_ENTITY_H +#define PROTOTYPE_LINK_EQUIPPED_ENTITY_H + +namespace argos { + class CPrototypeLinkEquippedEntity; + class CPrototypeLinkEntity; +} + +#include +#include +#include + +namespace argos { + + class CPrototypeLinkEquippedEntity : public CComposableEntity { + + public: + + ENABLE_VTABLE(); + + public: + + CPrototypeLinkEquippedEntity(CComposableEntity* pc_parent); + + CPrototypeLinkEquippedEntity(CComposableEntity* pc_parent, + const std::string& str_id); + + virtual ~CPrototypeLinkEquippedEntity() {} + + virtual void Init(TConfigurationNode& t_tree); + + CPrototypeLinkEntity& GetLink(UInt32 un_index); + + CPrototypeLinkEntity& GetLink(const std::string& str_id); + + inline CPrototypeLinkEntity::TVector& GetLinks() { + return m_vecLinks; + } + + CPrototypeLinkEntity& GetReferenceLink() { + return *m_vecLinks[0]; + } + + virtual std::string GetTypeDescription() const { + return "links"; + } + + private: + + CPrototypeLinkEntity::TVector m_vecLinks; + }; + +} + +#endif diff --git a/src/plugins/robots/prototype/simulator/qtopengl_prototype.cpp b/src/plugins/robots/prototype/simulator/qtopengl_prototype.cpp new file mode 100644 index 00000000..93e4afd6 --- /dev/null +++ b/src/plugins/robots/prototype/simulator/qtopengl_prototype.cpp @@ -0,0 +1,485 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#include "qtopengl_prototype.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace argos { + + /****************************************/ + /****************************************/ + + static const Real LED_RADIUS = 0.0025f; + static const Real FIELD_SCALE_FACTOR = 0.0005f; + const GLfloat BODY_COLOR[] = { 0.5f, 0.5f, 0.5f, 1.0f }; + const GLfloat SPECULAR[] = { 0.0f, 0.0f, 0.0f, 1.0f }; + const GLfloat SHININESS[] = { 0.0f }; + const GLfloat EMISSION[] = { 0.0f, 0.0f, 0.0f, 1.0f }; + + /****************************************/ + /****************************************/ + + CQTOpenGLPrototype::CQTOpenGLPrototype() : + m_unVertices(20) { + + /* Reserve the needed display lists */ + m_unBaseList = glGenLists(6); + /* References to the display lists */ + m_unBoxList = m_unBaseList; + m_unCylinderList = m_unBaseList + 1; + m_unSphereList = m_unBaseList + 2; + m_unLEDList = m_unBaseList + 3; + m_unPoleList = m_unBaseList + 4; + m_unTagList = m_unBaseList + 5; + /* Make box list */ + glNewList(m_unBoxList, GL_COMPILE); + MakeBox(); + glEndList(); + /* Make cylinder list */ + glNewList(m_unCylinderList, GL_COMPILE); + MakeCylinder(); + glEndList(); + /* Make sphere list */ + glNewList(m_unSphereList, GL_COMPILE); + MakeSphere(); + glEndList(); + /* Make LED list */ + glNewList(m_unLEDList, GL_COMPILE); + MakeLED(); + glEndList(); + /* Make Poles list */ + glNewList(m_unPoleList, GL_COMPILE); + MakePoles(); + glEndList(); + /* Make Tag list */ + MakeTagTexture(); + glNewList(m_unTagList, GL_COMPILE); + MakeTag(); + glEndList(); + } + + /****************************************/ + /****************************************/ + + CQTOpenGLPrototype::~CQTOpenGLPrototype() { + glDeleteLists(m_unBaseList, 6); + } + + /****************************************/ + /****************************************/ + + void CQTOpenGLPrototype::MakeLED() { + CVector3 cNormal, cPoint; + CRadians cSlice(CRadians::TWO_PI / m_unVertices); + + glBegin(GL_TRIANGLE_STRIP); + for(CRadians cInclination; cInclination <= CRadians::PI; cInclination += cSlice) { + for(CRadians cAzimuth; cAzimuth <= CRadians::TWO_PI; cAzimuth += cSlice) { + cNormal.FromSphericalCoords(1.0f, cInclination, cAzimuth); + cPoint = LED_RADIUS * cNormal; + glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ()); + glVertex3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ()); + cNormal.FromSphericalCoords(1.0f, cInclination + cSlice, cAzimuth); + cPoint = LED_RADIUS * cNormal; + glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ()); + glVertex3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ()); + cNormal.FromSphericalCoords(1.0f, cInclination, cAzimuth + cSlice); + cPoint = LED_RADIUS * cNormal; + glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ()); + glVertex3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ()); + cNormal.FromSphericalCoords(1.0f, cInclination + cSlice, cAzimuth + cSlice); + cPoint = LED_RADIUS * cNormal; + glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ()); + glVertex3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ()); + } + } + glEnd(); + } + + /****************************************/ + /****************************************/ + + void CQTOpenGLPrototype::DrawEntity(CPrototypeEntity& c_entity) { + /* Draw the links */ + for(CPrototypeLinkEntity* pcLink : c_entity.GetLinkEquippedEntity().GetLinks()){ + /* Configure the body material */ + glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, BODY_COLOR); +//#ifndef NDEBUG +// glPolygonMode(GL_FRONT, GL_LINE); +// glPolygonMode(GL_BACK, GL_LINE); +//#endif + /* Get the position of the link */ + const CVector3& cPosition = pcLink->GetAnchor().Position; + /* Get the orientation of the link */ + const CQuaternion& cOrientation = pcLink->GetAnchor().Orientation; + CRadians cZAngle, cYAngle, cXAngle; + cOrientation.ToEulerAngles(cZAngle, cYAngle, cXAngle); + glPushMatrix(); + glTranslatef(cPosition.GetX(), cPosition.GetY(), cPosition.GetZ()); + glRotatef(ToDegrees(cXAngle).GetValue(), 1.0f, 0.0f, 0.0f); + glRotatef(ToDegrees(cYAngle).GetValue(), 0.0f, 1.0f, 0.0f); + glRotatef(ToDegrees(cZAngle).GetValue(), 0.0f, 0.0f, 1.0f); + glScalef(pcLink->GetExtents().GetX(), + pcLink->GetExtents().GetY(), + pcLink->GetExtents().GetZ()); + /* Draw the link */ + switch(pcLink->GetGeometry()) { + case CPrototypeLinkEntity::EGeometry::BOX: + glCallList(m_unBoxList); + break; + case CPrototypeLinkEntity::EGeometry::CYLINDER: + glCallList(m_unCylinderList); + break; + case CPrototypeLinkEntity::EGeometry::SPHERE: + glCallList(m_unSphereList); + break; + } +//#ifndef NDEBUG +// glPolygonMode(GL_FRONT, GL_FILL); +// glPolygonMode(GL_BACK, GL_FILL); +//#endif + glPopMatrix(); + } + } + + /****************************************/ + /****************************************/ + + void CQTOpenGLPrototype::DrawDevices(CPrototypeEntity& c_entity) { + /* Draw the directional LEDs */ + if(c_entity.HasDirectionalLEDEquippedEntity()) { + GLfloat pfColor[] = { 0.0f, 0.0f, 0.0f, 1.0f }; + const GLfloat pfSpecular[] = { 0.0f, 0.0f, 0.0f, 1.0f }; + const GLfloat pfShininess[] = { 100.0f }; + const GLfloat pfEmission[] = { 0.0f, 0.0f, 0.0f, 1.0f }; + glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, pfSpecular); + glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, pfShininess); + glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, pfEmission); + const CDirectionalLEDEquippedEntity& cDirectionalLEDEquippedEntity = + c_entity.GetDirectionalLEDEquippedEntity(); + for(const CDirectionalLEDEquippedEntity::SInstance& s_instance : + cDirectionalLEDEquippedEntity.GetInstances()) { + glPushMatrix(); + /* Set the material */ + const CColor& cColor = s_instance.LED.GetColor(); + pfColor[0] = cColor.GetRed() / 255.0f; + pfColor[1] = cColor.GetGreen() / 255.0f; + pfColor[2] = cColor.GetBlue() / 255.0f; + glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pfColor); + /* Perform rototranslation */ + const CVector3& cPosition = s_instance.LED.GetPosition(); + glTranslatef(cPosition.GetX(), cPosition.GetY(), cPosition.GetZ()); + /* Draw the LED */ + glCallList(m_unLEDList); + glPopMatrix(); + } + } + /* Draw the LEDs */ + if(c_entity.HasLEDEquippedEntity()) { + GLfloat pfColor[] = { 0.0f, 0.0f, 0.0f, 1.0f }; + const GLfloat pfSpecular[] = { 0.0f, 0.0f, 0.0f, 1.0f }; + const GLfloat pfShininess[] = { 100.0f }; + const GLfloat pfEmission[] = { 0.0f, 0.0f, 0.0f, 1.0f }; + glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, pfSpecular); + glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, pfShininess); + glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, pfEmission); + for(const CLEDEquippedEntity::SActuator* ps_actuator : + c_entity.GetLEDEquippedEntity().GetLEDs()) { + glPushMatrix(); + /* Set the material */ + const CColor& cColor = ps_actuator->LED.GetColor(); + pfColor[0] = cColor.GetRed() / 255.0f; + pfColor[1] = cColor.GetGreen() / 255.0f; + pfColor[2] = cColor.GetBlue() / 255.0f; + glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pfColor); + /* Perform rototranslation */ + const CVector3& cPosition = ps_actuator->LED.GetPosition(); + glTranslatef(cPosition.GetX(), cPosition.GetY(), cPosition.GetZ()); + /* Draw the LED */ + glCallList(m_unLEDList); + glPopMatrix(); + } + } + /* Draw the tags */ + if(c_entity.HasTagEquippedEntity()) { + const CTagEquippedEntity& cTagEquippedEntity = + c_entity.GetTagEquippedEntity(); + CRadians cZ, cY, cX; + for(const CTagEquippedEntity::SInstance& s_instance : + cTagEquippedEntity.GetInstances()) { + const CVector3& cTagPosition = s_instance.Tag.GetPosition(); + const CQuaternion& cTagOrientation = s_instance.Tag.GetOrientation(); + cTagOrientation.ToEulerAngles(cZ, cY, cX); + Real fScaling = s_instance.Tag.GetSideLength(); + glPushMatrix(); + glTranslatef(cTagPosition.GetX(), + cTagPosition.GetY(), + cTagPosition.GetZ()); + glRotatef(ToDegrees(cX).GetValue(), 1.0f, 0.0f, 0.0f); + glRotatef(ToDegrees(cY).GetValue(), 0.0f, 1.0f, 0.0f); + glRotatef(ToDegrees(cZ).GetValue(), 0.0f, 0.0f, 1.0f); + glScalef(fScaling, fScaling, 1.0f); + glCallList(m_unTagList); + glPopMatrix(); + } + } + /* Draw the magnetic poles */ + if(c_entity.HasMagnetEquippedEntity()) { + CMagnetEquippedEntity& cMagnetEquippedEntity = + c_entity.GetMagnetEquippedEntity(); + CVector3 cFieldOrigin; + CQuaternion cFieldOrientation; + CRadians cFieldOrientationZ, cFieldOrientationY, cFieldOrientationX; + for(CMagnetEquippedEntity::SInstance& s_instance : + cMagnetEquippedEntity.GetInstances()) { + cFieldOrigin = s_instance.Offset; + cFieldOrigin.Rotate(s_instance.Anchor.Orientation); + cFieldOrigin += s_instance.Anchor.Position; + const CVector3& cField = s_instance.Magnet.GetField(); + cFieldOrientation = s_instance.Anchor.Orientation * + CQuaternion(CVector3::Z, cField); + cFieldOrientation.ToEulerAngles(cFieldOrientationZ, + cFieldOrientationY, + cFieldOrientationX); + glPushMatrix(); + glTranslatef(cFieldOrigin.GetX(), + cFieldOrigin.GetY(), + cFieldOrigin.GetZ()); + glRotatef(ToDegrees(cFieldOrientationX).GetValue(), 1.0f, 0.0f, 0.0f); + glRotatef(ToDegrees(cFieldOrientationY).GetValue(), 0.0f, 1.0f, 0.0f); + glRotatef(ToDegrees(cFieldOrientationZ).GetValue(), 0.0f, 0.0f, 1.0f); + glScalef(1.0f, 1.0f, cField.Length() * FIELD_SCALE_FACTOR); + glCallList(m_unPoleList); + glPopMatrix(); + } + } + } + + /****************************************/ + /****************************************/ + + void CQTOpenGLPrototype::MakeBox() { + glEnable(GL_NORMALIZE); + /* Set the material */ + glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, SPECULAR); + glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, SHININESS); + glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, EMISSION); + /* This part covers the top and bottom faces (parallel to XY) */ + glBegin(GL_QUADS); + /* Bottom face */ + glNormal3f(0.0f, 0.0f, -1.0f); + glVertex3f( 0.5f, 0.5f, 0.0f); + glVertex3f( 0.5f, -0.5f, 0.0f); + glVertex3f(-0.5f, -0.5f, 0.0f); + glVertex3f(-0.5f, 0.5f, 0.0f); + /* Top face */ + glNormal3f(0.0f, 0.0f, 1.0f); + glVertex3f(-0.5f, -0.5f, 1.0f); + glVertex3f( 0.5f, -0.5f, 1.0f); + glVertex3f( 0.5f, 0.5f, 1.0f); + glVertex3f(-0.5f, 0.5f, 1.0f); + glEnd(); + /* This part covers the faces (South, East, North, West) */ + glBegin(GL_QUADS); + /* South face */ + glNormal3f(0.0f, -1.0f, 0.0f); + glVertex3f(-0.5f, -0.5f, 1.0f); + glVertex3f(-0.5f, -0.5f, 0.0f); + glVertex3f( 0.5f, -0.5f, 0.0f); + glVertex3f( 0.5f, -0.5f, 1.0f); + /* East face */ + glNormal3f(1.0f, 0.0f, 0.0f); + glVertex3f( 0.5f, -0.5f, 1.0f); + glVertex3f( 0.5f, -0.5f, 0.0f); + glVertex3f( 0.5f, 0.5f, 0.0f); + glVertex3f( 0.5f, 0.5f, 1.0f); + /* North face */ + glNormal3f(0.0f, 1.0f, 0.0f); + glVertex3f( 0.5f, 0.5f, 1.0f); + glVertex3f( 0.5f, 0.5f, 0.0f); + glVertex3f(-0.5f, 0.5f, 0.0f); + glVertex3f(-0.5f, 0.5f, 1.0f); + /* West face */ + glNormal3f(-1.0f, 0.0f, 0.0f); + glVertex3f(-0.5f, 0.5f, 1.0f); + glVertex3f(-0.5f, 0.5f, 0.0f); + glVertex3f(-0.5f, -0.5f, 0.0f); + glVertex3f(-0.5f, -0.5f, 1.0f); + glEnd(); + glDisable(GL_NORMALIZE); + } + + /****************************************/ + /****************************************/ + + void CQTOpenGLPrototype::MakeCylinder() { + glEnable(GL_NORMALIZE); + /* Set the material */ + glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, SPECULAR); + glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, SHININESS); + glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, EMISSION); + /* Side surface */ + CVector2 cVertex(0.5f, 0.0f); + CRadians cAngle(CRadians::TWO_PI / m_unVertices); + glBegin(GL_QUAD_STRIP); + for(GLuint i = 0; i <= m_unVertices; i++) { + glNormal3f(cVertex.GetX(), cVertex.GetY(), 0.0f); + glVertex3f(cVertex.GetX(), cVertex.GetY(), 1.0f); + glVertex3f(cVertex.GetX(), cVertex.GetY(), 0.0f); + cVertex.Rotate(cAngle); + } + glEnd(); + /* Top disk */ + cVertex.Set(0.5f, 0.0f); + glBegin(GL_POLYGON); + glNormal3f(0.0f, 0.0f, 1.0f); + for(GLuint i = 0; i <= m_unVertices; i++) { + glVertex3f(cVertex.GetX(), cVertex.GetY(), 1.0f); + cVertex.Rotate(cAngle); + } + glEnd(); + /* Bottom disk */ + cVertex.Set(0.5f, 0.0f); + cAngle = -cAngle; + glBegin(GL_POLYGON); + glNormal3f(0.0f, 0.0f, -1.0f); + for(GLuint i = 0; i <= m_unVertices; i++) { + glVertex3f(cVertex.GetX(), cVertex.GetY(), 0.0f); + cVertex.Rotate(cAngle); + } + glEnd(); + glDisable(GL_NORMALIZE); + } + + /****************************************/ + /****************************************/ + + void CQTOpenGLPrototype::MakeSphere() { + glEnable(GL_NORMALIZE); + glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, SPECULAR); + glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, SHININESS); + glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, EMISSION); + CVector3 cNormal, cPoint; + CRadians cSlice(CRadians::TWO_PI / m_unVertices); + glBegin(GL_TRIANGLE_STRIP); + for(CRadians cInclination; cInclination <= CRadians::PI; cInclination += cSlice) { + for(CRadians cAzimuth; cAzimuth <= CRadians::TWO_PI; cAzimuth += cSlice) { + cPoint.FromSphericalCoords(0.5f, cInclination, cAzimuth); + glNormal3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f); + glVertex3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f); + cPoint.FromSphericalCoords(0.5f, cInclination + cSlice, cAzimuth); + glNormal3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f); + glVertex3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f); + cPoint.FromSphericalCoords(0.5f, cInclination, cAzimuth + cSlice); + glNormal3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f); + glVertex3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f); + cPoint.FromSphericalCoords(0.5f, cInclination + cSlice, cAzimuth + cSlice); + glNormal3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f); + glVertex3f(cPoint.GetX(), cPoint.GetY(), cPoint.GetZ() + 0.5f); + } + } + glEnd(); + glDisable(GL_NORMALIZE); + } + + /****************************************/ + /****************************************/ + + void CQTOpenGLPrototype::MakeTagTexture() { + glGenTextures(1, &m_unTagTex); + glBindTexture(GL_TEXTURE_2D, m_unTagTex); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 4, 4, 0, GL_RGB, GL_FLOAT, m_fTagTexture); + } + + /****************************************/ + /****************************************/ + + void CQTOpenGLPrototype::MakeTag() { + glEnable(GL_NORMALIZE); + glDisable(GL_LIGHTING); + glEnable(GL_TEXTURE_2D); + glBindTexture(GL_TEXTURE_2D, m_unTagTex); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + glBegin(GL_QUADS); + glNormal3f(0.0f, 0.0f, 1.0f); + glTexCoord2f(1.0f, 1.0f); glVertex2f( 0.5f, 0.5f); + glTexCoord2f(0.03f, 1.0f); glVertex2f(-0.5f, 0.5f); + glTexCoord2f(0.03f, 0.03f); glVertex2f(-0.5f, -0.5f); + glTexCoord2f(1.0f, 0.03f); glVertex2f( 0.5f, -0.5f); + glEnd(); + glDisable(GL_TEXTURE_2D); + glEnable(GL_LIGHTING); + glDisable(GL_NORMALIZE); + } + + /****************************************/ + /****************************************/ + + void CQTOpenGLPrototype::MakePoles() { + glEnable(GL_NORMALIZE); + glDisable(GL_LIGHTING); + glLineWidth(4.0f); + glBegin(GL_LINES); + /* south pole */ + glColor3f(1.0, 0.0, 0.0); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 0.5f); + /* north pole */ + glColor3f(0.0, 0.0, 1.0); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 0.0f, -0.5f); + glEnd(); + glLineWidth(1.0f); + glEnable(GL_LIGHTING); + glDisable(GL_NORMALIZE); + } + + /****************************************/ + /****************************************/ + + class CQTOpenGLOperationDrawPrototypeNormal : public CQTOpenGLOperationDrawNormal { + public: + void ApplyTo(CQTOpenGLWidget& c_visualization, + CPrototypeEntity& c_entity) { + static CQTOpenGLPrototype m_cModel; + if(c_entity.HasControllableEntity()) { + c_visualization.DrawRays(c_entity.GetControllableEntity()); + } + m_cModel.DrawEntity(c_entity); + m_cModel.DrawDevices(c_entity); + } + }; + + /****************************************/ + /****************************************/ + + class CQTOpenGLOperationDrawPrototypeSelected : public CQTOpenGLOperationDrawSelected { + public: + void ApplyTo(CQTOpenGLWidget& c_visualization, + CPrototypeEntity& c_entity) { + c_visualization.DrawBoundingBox(c_entity.GetEmbodiedEntity()); + } + }; + + REGISTER_ENTITY_OPERATION(CQTOpenGLOperationDrawNormal, CQTOpenGLWidget, CQTOpenGLOperationDrawPrototypeNormal, void, CPrototypeEntity); + + REGISTER_ENTITY_OPERATION(CQTOpenGLOperationDrawSelected, CQTOpenGLWidget, CQTOpenGLOperationDrawPrototypeSelected, void, CPrototypeEntity); + + /****************************************/ + /****************************************/ + +} diff --git a/src/plugins/robots/prototype/simulator/qtopengl_prototype.h b/src/plugins/robots/prototype/simulator/qtopengl_prototype.h new file mode 100644 index 00000000..0809220f --- /dev/null +++ b/src/plugins/robots/prototype/simulator/qtopengl_prototype.h @@ -0,0 +1,71 @@ +/** + * @file + * + * @author Michael Allwright - + */ + +#ifndef QTOPENGL_PROTOTYPE_H +#define QTOPENGL_PROTOTYPE_H + +namespace argos { + class CPrototypeEntity; +} + +#ifdef __APPLE__ +#include +#else +#include +#endif + +namespace argos { + + class CQTOpenGLPrototype { + + public: + + CQTOpenGLPrototype(); + + virtual ~CQTOpenGLPrototype(); + + virtual void DrawEntity(CPrototypeEntity& c_entity); + + virtual void DrawDevices(CPrototypeEntity& c_entity); + + private: + + void MakeBox(); + void MakeCylinder(); + void MakeSphere(); + void MakeLED(); + void MakePoles(); + void MakeTag(); + void MakeTagTexture(); + + private: + + GLuint m_unBaseList; + GLuint m_unBoxList; + GLuint m_unCylinderList; + GLuint m_unSphereList; + GLuint m_unLEDList; + GLuint m_unPoleList; + GLuint m_unTagList; + GLuint m_unVertices; + GLuint m_unTagTex; + + const GLfloat m_fTagTexture[48] = { + 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, + 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, + /* shade the bottom corner in debug mode */ +#ifndef NDEBUG + 0.5f, 0.5f, 0.5f +#else + 1.0f, 1.0f, 1.0f +#endif + }; + }; +} + +#endif diff --git a/src/scripts/gdbinit.in b/src/scripts/gdbinit.in index 0387f77d..a33cfb53 100644 --- a/src/scripts/gdbinit.in +++ b/src/scripts/gdbinit.in @@ -1,2 +1,2 @@ -set environment ARGOS_PLUGIN_PATH @CMAKE_BINARY_DIR@/core:@CMAKE_BINARY_DIR@/testing:@CMAKE_BINARY_DIR@/plugins/robots/e-puck:@CMAKE_BINARY_DIR@/plugins/robots/eye-bot:@CMAKE_BINARY_DIR@/plugins/robots/foot-bot:@CMAKE_BINARY_DIR@/plugins/robots/generic:@CMAKE_BINARY_DIR@/plugins/robots/spiri:@CMAKE_BINARY_DIR@/plugins/simulator/visualizations/qt-opengl:@CMAKE_BINARY_DIR@/plugins/simulator/entities:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/dynamics2d:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/dynamics3d:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/physx:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/pointmass3d:@CMAKE_BINARY_DIR@/plugins/simulator/media +set environment ARGOS_PLUGIN_PATH @CMAKE_BINARY_DIR@/core:@CMAKE_BINARY_DIR@/testing:@CMAKE_BINARY_DIR@/plugins/robots/e-puck:@CMAKE_BINARY_DIR@/plugins/robots/eye-bot:@CMAKE_BINARY_DIR@/plugins/robots/foot-bot:@CMAKE_BINARY_DIR@/plugins/robots/generic:@CMAKE_BINARY_DIR@/plugins/robots/prototype:@CMAKE_BINARY_DIR@/plugins/robots/spiri:@CMAKE_BINARY_DIR@/plugins/simulator/visualizations/qt-opengl:@CMAKE_BINARY_DIR@/plugins/simulator/entities:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/dynamics2d:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/dynamics3d:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/physx:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/pointmass3d:@CMAKE_BINARY_DIR@/plugins/simulator/media set environment LD_LIBRARY_PATH ${ARGOS_PLUGIN_PATH} diff --git a/src/scripts/setup_env.sh.in b/src/scripts/setup_env.sh.in index ee0ddee6..e5784334 100644 --- a/src/scripts/setup_env.sh.in +++ b/src/scripts/setup_env.sh.in @@ -1,5 +1,5 @@ #!/bin/sh -export ARGOS_PLUGIN_PATH=@CMAKE_BINARY_DIR@/core:@CMAKE_BINARY_DIR@/testing:@CMAKE_BINARY_DIR@/plugins/robots/e-puck:@CMAKE_BINARY_DIR@/plugins/robots/eye-bot:@CMAKE_BINARY_DIR@/plugins/robots/foot-bot:@CMAKE_BINARY_DIR@/plugins/robots/generic:@CMAKE_BINARY_DIR@/plugins/robots/spiri:@CMAKE_BINARY_DIR@/plugins/simulator/visualizations/qt-opengl:@CMAKE_BINARY_DIR@/plugins/simulator/entities:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/dynamics2d:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/dynamics3d:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/physx:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/pointmass3d:@CMAKE_BINARY_DIR@/plugins/simulator/media +export ARGOS_PLUGIN_PATH=@CMAKE_BINARY_DIR@/core:@CMAKE_BINARY_DIR@/testing:@CMAKE_BINARY_DIR@/plugins/robots/e-puck:@CMAKE_BINARY_DIR@/plugins/robots/eye-bot:@CMAKE_BINARY_DIR@/plugins/robots/foot-bot:@CMAKE_BINARY_DIR@/plugins/robots/generic:@CMAKE_BINARY_DIR@/plugins/robots/prototype:@CMAKE_BINARY_DIR@/plugins/robots/spiri:@CMAKE_BINARY_DIR@/plugins/simulator/visualizations/qt-opengl:@CMAKE_BINARY_DIR@/plugins/simulator/entities:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/dynamics2d:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/dynamics3d:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/physx:@CMAKE_BINARY_DIR@/plugins/simulator/physics_engines/pointmass3d:@CMAKE_BINARY_DIR@/plugins/simulator/media export @ARGOS_DYNAMIC_LIBRARY_VARIABLE@=${@ARGOS_DYNAMIC_LIBRARY_VARIABLE@}:${ARGOS_PLUGIN_PATH}