diff --git a/adf_loader/version_1_0/adf_loader_1_0.cpp b/adf_loader/version_1_0/adf_loader_1_0.cpp index 10345f853..b3c011fa8 100644 --- a/adf_loader/version_1_0/adf_loader_1_0.cpp +++ b/adf_loader/version_1_0/adf_loader_1_0.cpp @@ -1561,6 +1561,8 @@ bool ADFLoader_1_0::loadRayTracerSensorAttribs(YAML::Node *a_node, afRayTracerSe return 0; } + attribs->m_sensorType = afSensorType::RAYTRACER; + bool result = true; // Declare all the yaml parameters that we want to look for YAML::Node parentNameNode = node["parent"]; @@ -1695,6 +1697,8 @@ bool ADFLoader_1_0::loadResistanceSensorAttribs(YAML::Node *a_node, afResistance bool result = false; result = loadRayTracerSensorAttribs(a_node, attribs); + attribs->m_sensorType = afSensorType::RESISTANCE; + if (result){ YAML::Node resistiveFrictionNode = node["friction"]; @@ -2146,14 +2150,14 @@ bool ADFLoader_1_0::loadModelAttribs(YAML::Node *a_node, afModelAttributes *attr afSensorType senType = ADFUtils::getSensorTypeFromString(senNode["type"].as()); switch (senType) { case afSensorType::RAYTRACER:{ - afRayTracerSensorAttributes senAttribs; - loadRayTracerSensorAttribs(&senNode, &senAttribs); + afRayTracerSensorAttributes* senAttribs = new afRayTracerSensorAttributes(); + loadRayTracerSensorAttribs(&senNode, senAttribs); attribs->m_sensorAttribs.push_back(senAttribs); break; } case afSensorType::RESISTANCE:{ - afResistanceSensorAttributes senAttribs; - loadResistanceSensorAttribs(&senNode, &senAttribs); + afResistanceSensorAttributes* senAttribs = new afResistanceSensorAttributes(); + loadResistanceSensorAttribs(&senNode, senAttribs); attribs->m_sensorAttribs.push_back(senAttribs); break; } @@ -2174,8 +2178,8 @@ bool ADFLoader_1_0::loadModelAttribs(YAML::Node *a_node, afModelAttributes *attr // Check if this is a constraint sensor switch (actType) { case afActuatorType::CONSTRAINT:{ - afConstraintActuatorAttributes acAttribs; - loadConstraintActuatorAttribs(&actNode, &acAttribs); + afConstraintActuatorAttributes* acAttribs = new afConstraintActuatorAttributes(); + loadConstraintActuatorAttribs(&actNode, acAttribs); attribs->m_actuatorAttribs.push_back(acAttribs); break; } diff --git a/ambf_framework/afAttributes.h b/ambf_framework/afAttributes.h index 76e4b79e4..fa9288087 100644 --- a/ambf_framework/afAttributes.h +++ b/ambf_framework/afAttributes.h @@ -822,8 +822,8 @@ struct afModelAttributes: public afBaseObjectAttributes, public afFileObjectAttr std::vector m_softBodyAttribs; std::vector m_vehicleAttribs; std::vector m_jointAttribs; - std::vector m_sensorAttribs; - std::vector m_actuatorAttribs; + std::vector m_sensorAttribs; + std::vector m_actuatorAttribs; bool m_ignoreInterCollision; @@ -848,11 +848,11 @@ struct afModelAttributes: public afBaseObjectAttributes, public afFileObjectAttr } for (int i = 0 ; i < m_sensorAttribs.size() ; i++){ - m_sensorAttribs[i].resolveRelativeNamespace(a_parentNamespace); + m_sensorAttribs[i]->resolveRelativeNamespace(a_parentNamespace); } for (int i = 0 ; i < m_actuatorAttribs.size() ; i++){ - m_actuatorAttribs[i].resolveRelativeNamespace(a_parentNamespace); + m_actuatorAttribs[i]->resolveRelativeNamespace(a_parentNamespace); } m_namespaceResolved = true; @@ -877,11 +877,11 @@ struct afModelAttributes: public afBaseObjectAttributes, public afFileObjectAttr } for (int i = 0 ; i < m_sensorAttribs.size() ; i++){ - m_sensorAttribs[i].resolveRelativePathAttribs(a_parentPath); + m_sensorAttribs[i]->resolveRelativePathAttribs(a_parentPath); } for (int i = 0 ; i < m_actuatorAttribs.size() ; i++){ - m_actuatorAttribs[i].resolveRelativePathAttribs(a_parentPath); + m_actuatorAttribs[i]->resolveRelativePathAttribs(a_parentPath); } m_pathsResolved = true; diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp index 930a36f20..04a69f9ad 100644 --- a/ambf_framework/afFramework.cpp +++ b/ambf_framework/afFramework.cpp @@ -864,9 +864,13 @@ void afBaseObject::setLocalTransform(const afTransform &trans) m_localTransform << trans; } -void afBaseObject::addChildObject(afBaseObjectPtr a_afObject) +bool afBaseObject::addChildObject(afBaseObjectPtr a_afObject) { + if (a_afObject == this){ + return false; + } a_afObject->setParentObject(this); + return true; } @@ -884,9 +888,9 @@ void afBaseObject::setParentObject(afBaseObject *a_afObject) /// \brief afBaseObject::toggleFrameVisibility /// void afBaseObject::toggleFrameVisibility(){ - std::vector::iterator it; + std::vector::iterator it; for (it = m_childrenSceneObjects.begin(); it != m_childrenSceneObjects.end() ; ++it){ - (*it)->setShowFrame(!(*it)->getShowFrame()); + (*it)->getChaiObject()->setShowFrame(!(*it)->getChaiObject()->getShowFrame()); } // if (m_visualMesh){ // m_visualMesh->setShowFrame(!m_visualMesh->getShowFrame()); @@ -899,9 +903,8 @@ void afBaseObject::toggleFrameVisibility(){ /// \param a_object /// \return /// -bool afBaseObject::isSceneObjectAlreadyAdded(cGenericObject *a_object) -{ - vector::iterator it; +bool afBaseObject::isSceneObjectAlreadyAdded(afSceneObject *a_object){ + vector::iterator it; for(it = m_childrenSceneObjects.begin() ; it != m_childrenSceneObjects.end() ; ++it){ if ((*it) == a_object){ return true; @@ -916,23 +919,32 @@ bool afBaseObject::isSceneObjectAlreadyAdded(cGenericObject *a_object) /// \brief afBaseObject::setScale /// \param a_scale /// -void afBaseObject::setScale(double a_scale) -{ +void afBaseObject::setScale(double a_scale){ m_scale = a_scale; scaleSceneObjects(m_scale); } - - /// /// \brief afBaseObject::addChild /// \param a_visualMesh /// -bool afBaseObject::addChildSceneObject(cGenericObject *a_object) +bool afBaseObject::addChildSceneObject(cGenericObject *a_object){ + + if (a_object == nullptr){ + return false; + } + + afSceneObject* sceneObject = new afSceneObject; + sceneObject->setChaiObject(a_object); + sceneObject->setOffsetTransform(a_object->getLocalTransform()); + return addChildSceneObject(sceneObject); +} + +bool afBaseObject::addChildSceneObject(afSceneObject *a_object) { // sanity check - if (a_object == NULL){ + if (a_object == nullptr){ return false; } @@ -943,19 +955,40 @@ bool afBaseObject::addChildSceneObject(cGenericObject *a_object) // the object does not have any parent yet, so we can add it as a child // to current object. - m_childrenSceneObjects.push_back(a_object); return true; } -void afBaseObject::scaleSceneObjects(double a_scale) +void afBaseObject::scaleSceneObjects(double a_scale){ + std::vector::iterator it; + for (it = m_childrenSceneObjects.begin(); it != m_childrenSceneObjects.end(); ++it){ + (*it)->getChaiObject()->scale(a_scale); + + } +} + +bool afBaseObject::removeChildSceneObject(cGenericObject *a_object, bool removeFromGraph) { - std::vector::iterator it; + // sanity check + if (a_object == nullptr) { return (false); } + + vector::iterator it; for (it = m_childrenSceneObjects.begin(); it != m_childrenSceneObjects.end(); ++it) { - (*it)->scale(a_scale); - + if ((*it)->getChaiObject() == a_object) + { + if (removeFromGraph){ + (*it)->getChaiObject()->removeFromGraph(); + } + // remove this object from my list of children + m_childrenSceneObjects.erase(it); + // return success + return true; + } } + + // operation failed + return false; } @@ -963,18 +996,17 @@ void afBaseObject::scaleSceneObjects(double a_scale) /// \brief afBaseObject::removeChild /// \param a_cObject /// -bool afBaseObject::removeChildSceneObject(cGenericObject *a_object, bool removeFromGraph) -{ +bool afBaseObject::removeChildSceneObject(afSceneObject *a_object, bool removeFromGraph){ // sanity check - if (a_object == NULL) { return (false); } + if (a_object == nullptr) { return (false); } - vector::iterator it; + vector::iterator it; for (it = m_childrenSceneObjects.begin(); it != m_childrenSceneObjects.end(); ++it) { if ((*it) == a_object) { if (removeFromGraph){ - (*it)->removeFromGraph(); + (*it)->getChaiObject()->removeFromGraph(); } // remove this object from my list of children m_childrenSceneObjects.erase(it); @@ -984,29 +1016,27 @@ bool afBaseObject::removeChildSceneObject(cGenericObject *a_object, bool removeF } // operation failed - return (false); + return false; } -void afBaseObject::removeAllChildSceneObjects(bool removeFromGraph) -{ - std::vector::iterator it; +void afBaseObject::removeAllChildSceneObjects(bool removeFromGraph){ + std::vector::iterator it; for (it = m_childrenSceneObjects.begin() ; it != m_childrenSceneObjects.end() ; ++it){ removeChildSceneObject((*it), removeFromGraph); } } -void afBaseObject::updateSceneObjects() -{ +void afBaseObject::updateSceneObjects(){ // Assuming that the global pose was computed prior to this call. - vector::iterator it; + vector::iterator it; for (it = m_childrenSceneObjects.begin() ; it != m_childrenSceneObjects.end() ; ++it){ - (*it)->setLocalTransform(m_globalTransform); + cTransform globalTrans = m_globalTransform * (*it)->getOffsetTransform(); + (*it)->getChaiObject()->setLocalTransform(globalTrans); } } -void afBaseObject::updateGlobalPose() -{ +void afBaseObject::updateGlobalPose(){ cTransform a_globalTransform = m_localTransform; // Traverse up the parents to resolve the global pose @@ -1023,10 +1053,10 @@ void afBaseObject::updateGlobalPose() void afBaseObject::showVisualFrame() { - std::vector::iterator it; + std::vector::iterator it; for (it = m_childrenSceneObjects.begin(); it != m_childrenSceneObjects.end() ; ++it){ // Set the size of the frame. - cVector3d bounds = (*it)->getBoundaryMax(); + cVector3d bounds = (*it)->getChaiObject()->getBoundaryMax(); double frame_size; if (bounds.length() > 0.001){ double max_axis = cMax3(bounds.x(), bounds.y(), bounds.z()); @@ -1035,7 +1065,7 @@ void afBaseObject::showVisualFrame() else{ frame_size = 0.5; } - (*it)->setFrameSize(frame_size); + (*it)->getChaiObject()->setFrameSize(frame_size); } } @@ -1114,6 +1144,8 @@ bool afConstraintActuator::createFromAttribs(afConstraintActuatorAttributes *a_a m_parentBody->addAFActuator(this); + setParentObject(m_parentBody); + m_maxImpulse = attribs.m_maxImpulse; m_tau = attribs.m_tau; @@ -3156,9 +3188,17 @@ afRayTracerSensor::afRayTracerSensor(afWorldPtr a_afWorld, afModelPtr a_modelPtr } -bool afRayTracerSensor::createFromAttribs(afRayTracerSensorAttributes *a_attribs) +void afRayTracerResult::initMeshes() { + m_fromSphereMesh = new cMesh; + m_toSphereMesh = new cMesh; + m_hitNormalMesh = new cMesh; + m_hitSphereMesh = new cMesh; +} + +bool afRayTracerSensor::createFromAttribs(afRayTracerSensorAttributes *a_attribs) +{ afRayTracerSensorAttributes &attribs = *a_attribs; bool result = true; @@ -3194,15 +3234,15 @@ bool afRayTracerSensor::createFromAttribs(afRayTracerSensorAttributes *a_attribs } } - m_parentBody->addAFSensor(this); - - + m_parentBody->addChildObject(this); switch (attribs.m_specificationType) { case afSensactorSpecificationType::ARRAY: case afSensactorSpecificationType::PARAMETRIC:{ + m_count = attribs.m_raysAttribs.size(); m_raysAttribs = attribs.m_raysAttribs; + m_rayTracerResults.resize(m_count); break; } case afSensactorSpecificationType::MESH:{ @@ -3281,10 +3321,10 @@ void afRayTracerSensor::update(){ rayToWorld << T_bInw * to_cVector3d(m_raysAttribs[i].m_rayToLocal); // Check for global flag for debug visibility of this sensor - if (m_showSensor){ - m_rayTracerResults[i].m_fromSphereMesh->setLocalPos(to_cVector3d(rayFromWorld)); - m_rayTracerResults[i].m_toSphereMesh->setLocalPos(to_cVector3d(rayToWorld)); - } +// if (m_showSensor){ +// m_rayTracerResults[i].m_fromSphereMesh->setLocalPos(to_cVector3d(rayFromWorld)); +// m_rayTracerResults[i].m_toSphereMesh->setLocalPos(to_cVector3d(rayToWorld)); +// } btCollisionWorld::ClosestRayResultCallback rayCallBack(rayFromWorld, rayToWorld); m_afWorld->m_bulletWorld->rayTest(rayFromWorld, rayToWorld, rayCallBack); @@ -3430,34 +3470,39 @@ void afRayTracerSensor::enableVisualization(){ if (m_rayTracerResults[i].m_hitSphereMesh == nullptr){ cMesh* mesh = new cMesh(); cCreateSphere(mesh, m_visibilitySphereRadius); - m_afWorld->addSceneObjectToWorld(mesh); mesh->m_material->setPinkHot(); mesh->setShowEnabled(false); mesh->setUseDisplayList(true); mesh->markForUpdate(false); m_rayTracerResults[i].m_hitSphereMesh = mesh; + addChildSceneObject(mesh); + m_afWorld->addSceneObjectToWorld(mesh); } if (m_rayTracerResults[i].m_fromSphereMesh == nullptr){ cMesh* mesh = new cMesh(); cCreateSphere(mesh, m_visibilitySphereRadius); - m_afWorld->addSceneObjectToWorld(mesh); mesh->m_material->setRed(); mesh->setShowEnabled(true); mesh->setUseDisplayList(true); mesh->markForUpdate(false); m_rayTracerResults[i].m_fromSphereMesh = mesh; + mesh->setLocalPos(to_cVector3d(m_raysAttribs[i].m_rayFromLocal)); + addChildSceneObject(mesh); + m_afWorld->addSceneObjectToWorld(mesh); } if (m_rayTracerResults[i].m_toSphereMesh == nullptr){ cMesh* mesh = new cMesh(); cCreateSphere(mesh, m_visibilitySphereRadius); - m_afWorld->addSceneObjectToWorld(mesh); mesh->m_material->setGreen(); mesh->setShowEnabled(true); mesh->setUseDisplayList(true); mesh->markForUpdate(false); m_rayTracerResults[i].m_toSphereMesh = mesh; + mesh->setLocalPos(to_cVector3d(m_raysAttribs[i].m_rayToLocal)); + addChildSceneObject(mesh); + m_afWorld->addSceneObjectToWorld(mesh); } if (m_rayTracerResults[i].m_hitNormalMesh == nullptr){ @@ -3467,12 +3512,13 @@ void afRayTracerSensor::enableVisualization(){ m_visibilitySphereRadius*1, m_visibilitySphereRadius*0.8, false); - m_afWorld->addSceneObjectToWorld(mesh); mesh->m_material->setGreenForest(); mesh->setShowEnabled(false); mesh->setUseDisplayList(true); mesh->markForUpdate(false); m_rayTracerResults[i].m_hitNormalMesh = mesh; + addChildSceneObject(mesh); + m_afWorld->addSceneObjectToWorld(mesh); } } } @@ -6268,18 +6314,23 @@ bool afModel::createFromAttribs(afModelAttributes *a_attribs) for (size_t i = 0; i < attribs.m_sensorAttribs.size(); ++i) { afSensorPtr sensorPtr = nullptr; string type_str; + bool valid = false; // Check which type of sensor is this so we can cast appropriately beforehand - switch (attribs.m_sensorAttribs[i].m_sensorType) { + switch (attribs.m_sensorAttribs[i]->m_sensorType) { case afSensorType::RAYTRACER: { sensorPtr = new afProximitySensor(m_afWorld, this); - type_str = "RESISTANCE"; + type_str = "PROXIMITY"; + afRayTracerSensorAttributes* senAttribs = (afRayTracerSensorAttributes*) attribs.m_sensorAttribs[i]; + valid = ((afRayTracerSensor*)sensorPtr)->createFromAttribs(senAttribs); break; } case afSensorType::RESISTANCE: { sensorPtr = new afResistanceSensor(m_afWorld, this); type_str = "RESISTANCE"; + afResistanceSensorAttributes* senAttribs = (afResistanceSensorAttributes*) attribs.m_sensorAttribs[i]; + valid = ((afResistanceSensor*)sensorPtr)->createFromAttribs(senAttribs); break; } default: @@ -6288,7 +6339,7 @@ bool afModel::createFromAttribs(afModelAttributes *a_attribs) // Finally load the sensor's attribs if (sensorPtr){ - if (sensorPtr->createFromAttribs(&attribs.m_sensorAttribs[i])){ + if (valid){ string remaped_name = m_afWorld->addAFSensor(sensorPtr); if (enable_comm){ cerr << "LOADING SENSOR COMM \n"; @@ -6312,10 +6363,13 @@ bool afModel::createFromAttribs(afModelAttributes *a_attribs) for (size_t i = 0; i < attribs.m_actuatorAttribs.size(); ++i) { afActuatorPtr actuatorPtr = nullptr; string type_str; - switch (attribs.m_actuatorAttribs[i].m_actuatorType) { + bool valid = false; + switch (attribs.m_actuatorAttribs[i]->m_actuatorType) { case afActuatorType::CONSTRAINT:{ actuatorPtr = new afConstraintActuator(m_afWorld, this); type_str = "CONSTRAINT"; + afConstraintActuatorAttributes* actAttribs = (afConstraintActuatorAttributes*)attribs.m_actuatorAttribs[i]; + valid = ((afConstraintActuator*)actuatorPtr)->createFromAttribs(actAttribs); break; } default: @@ -6324,7 +6378,7 @@ bool afModel::createFromAttribs(afModelAttributes *a_attribs) // Finally load the sensor from ambf config data if (actuatorPtr){ - if (actuatorPtr->createFromAttribs(&attribs.m_actuatorAttribs[i])){ + if (valid){ string remaped_name = m_afWorld->addAFActuator(actuatorPtr); if (enable_comm){ cerr << "LOADING ACTUATOR COMM \n"; diff --git a/ambf_framework/afFramework.h b/ambf_framework/afFramework.h index 48c98fa0b..05e3879b6 100644 --- a/ambf_framework/afFramework.h +++ b/ambf_framework/afFramework.h @@ -427,14 +427,17 @@ class afJointController: public afJointControllerAttributes{ /// struct afRayTracerResult{ +public: + void initMeshes(); + // The rigid body that this proximity sensor is sensing - btRigidBody* m_sensedBTRigidBody; + btRigidBody* m_sensedBTRigidBody = nullptr; // This is the AF Rigid body sensed by this sensor afRigidBodyPtr m_sensedAFRigidBody = nullptr; // The soft body that this proximity sensor is sensing - btSoftBody* m_sensedBTSoftBody; + btSoftBody* m_sensedBTSoftBody = nullptr; // This is the AF Soft body sensed by this sensor afSoftBodyPtr m_sensedAFSoftBody = nullptr; @@ -446,10 +449,10 @@ struct afRayTracerResult{ int m_sensedSoftBodyNodeIdx = -1; // The node ptr to the sensed soft body's face - btSoftBody::Face* m_sensedSoftBodyFace; + btSoftBody::Face* m_sensedSoftBodyFace = nullptr; // The node ptr to the sensed soft body's node - btSoftBody::Node* m_sensedSoftBodyNode; + btSoftBody::Node* m_sensedSoftBodyNode = nullptr; // Boolean for sensor sensing something bool m_triggered; @@ -458,13 +461,17 @@ struct afRayTracerResult{ cVector3d m_sensedLocationWorld; // Visual markers to show the hit point and the sensor start and end points - cMesh *m_hitSphereMesh, *m_fromSphereMesh, *m_toSphereMesh; + cMesh *m_hitSphereMesh = nullptr; + + cMesh *m_fromSphereMesh = nullptr; + + cMesh *m_toSphereMesh = nullptr; // Visual Mesh for Normal at Contact Point cMesh *m_hitNormalMesh; // Internal constraint for rigid body gripping - btPoint2PointConstraint* _p2p; + btPoint2PointConstraint *pointConstraint = nullptr; // Depth fraction is the normalized penetration depth of the sensor double m_depthFraction = 0; @@ -514,6 +521,29 @@ struct afChildJointPair{ }; +class afSceneObject{ + friend class afBaseObject; +public: + afSceneObject(){ + m_offsetTransform.identity(); + } + afSceneObject(cTransform& a_trans){ + m_offsetTransform = a_trans; + } + + inline cTransform getOffsetTransform(){return m_offsetTransform;} + + void setOffsetTransform(const cTransform &a_trans){m_offsetTransform = a_trans;} + + inline cGenericObject* getChaiObject(){return m_chaiObject;} + + void setChaiObject(cGenericObject* a_object){m_chaiObject = a_object;} + +protected: + cGenericObject* m_chaiObject; + cTransform m_offsetTransform; +}; + class afBaseObject: public afComm{ @@ -565,7 +595,7 @@ class afBaseObject: public afComm{ void setParentObject(afBaseObjectPtr a_afObject); - void addChildObject(afBaseObjectPtr a_afObject); + bool addChildObject(afBaseObjectPtr a_afObject); inline void setInitialTransform(cTransform a_trans){m_initialTransform = a_trans;} @@ -574,13 +604,17 @@ class afBaseObject: public afComm{ // This method toggles the viewing of frames of this rigid body. void toggleFrameVisibility(); - bool isSceneObjectAlreadyAdded(cGenericObject* a_object); + bool isSceneObjectAlreadyAdded(afSceneObject* a_object); bool addChildSceneObject(cGenericObject* a_object); + bool addChildSceneObject(afSceneObject* a_object); + void scaleSceneObjects(double a_scale); - bool removeChildSceneObject(cGenericObject* a_cObject, bool removeFromGraph); + bool removeChildSceneObject(cGenericObject* a_object, bool removeFromGraph); + + bool removeChildSceneObject(afSceneObject* a_object, bool removeFromGraph); void removeAllChildSceneObjects(bool removeFromGraphs=true); @@ -614,7 +648,7 @@ class afBaseObject: public afComm{ cMultiMesh* m_visualMesh; - std::vector m_childrenSceneObjects; + std::vector m_childrenSceneObjects; vector m_afChildrenObjects; @@ -1083,6 +1117,8 @@ class afActuator: public afBaseObject{ public: afActuator(afWorldPtr a_afWorld, afModelPtr a_modelPtr); + virtual bool createFromAttribs(afActuatorAttributes *a_attribs){return false;} + virtual void actuate(){} virtual void deactuate(){} @@ -1167,6 +1203,8 @@ class afSensor: public afBaseObject{ public: afSensor(afWorldPtr a_afWorld, afModelPtr a_modelPtr); + virtual bool createFromAttribs(afSensorAttributes* a_attribs){return false;} + // Toggle the debug display of the sensor inline void toggleSensorVisibility() {m_showSensor = !m_showSensor; }