diff --git a/graphics/src/wrapper/OSGNodeStruct.cpp b/graphics/src/wrapper/OSGNodeStruct.cpp index 8c2ff21c8..a07343769 100644 --- a/graphics/src/wrapper/OSGNodeStruct.cpp +++ b/graphics/src/wrapper/OSGNodeStruct.cpp @@ -82,51 +82,47 @@ namespace mars { } if (filename.compare("PRIMITIVE") == 0) { Vector vizSize = node.ext; - switch(NodeData::typeFromString(origname.c_str())) { - case mars::interfaces::NODE_TYPE_BOX: { + if (origname == "box") { drawObject_ = new CubeDrawObject(g); - break; } - case mars::interfaces::NODE_TYPE_SPHERE: { + else if (origname == "sphere"){ vizSize.x() *= 2; vizSize.y() = vizSize.z() = vizSize.x(); drawObject_ = new SphereDrawObject(g); - break; } - case mars::interfaces::NODE_TYPE_REFERENCE: { + else if (origname == "reference"){ #warning add here an coordinate system item //For now until we have an real coordinate system drawObject_ = new SphereDrawObject(g); - break; } - case mars::interfaces::NODE_TYPE_MESH: - case mars::interfaces::NODE_TYPE_CYLINDER: + else if (origname == "mesh"){ + + } + else if (origname == "cylinder"){ vizSize.x() *= 2; vizSize.z() = vizSize.y(); vizSize.y() = vizSize.x(); drawObject_ = new CylinderDrawObject(g, 1, 1); - break; - case mars::interfaces::NODE_TYPE_CAPSULE: { + } + else if (origname == "capsule"){ vizSize.x() *= 2; vizSize.z() = vizSize.y(); vizSize.y() = vizSize.x(); drawObject_ = new CapsuleDrawObject(g); - break; } - case mars::interfaces::NODE_TYPE_PLANE: { + else if (origname == "plane"){ drawObject_ = new PlaneDrawObject(g, vizSize); - break; } - case mars::interfaces::NODE_TYPE_EMPTY: { + else if (origname == "empty"){ drawObject_ = new EmptyDrawObject(g); - break; } - default: - fprintf(stderr,"Cannot find primitive type: %i(%s), at %s:%i\n", - NodeData::typeFromString(origname.c_str()), + else { + fprintf(stderr,"Cannot find primitive type: %s(%s), at %s:%i\n", + node.nodeType.c_str(), origname.c_str(), __FILE__, __LINE__); throw std::runtime_error("unknown primitive type"); } + if(map.find("maxNumLights") != map.end()) { drawObject_->setMaxNumLights(map["maxNumLights"]); } diff --git a/interfaces/src/NodeData.cpp b/interfaces/src/NodeData.cpp index a2f9d1d74..c00d4a19f 100644 --- a/interfaces/src/NodeData.cpp +++ b/interfaces/src/NodeData.cpp @@ -52,7 +52,7 @@ namespace mars { namespace interfaces { using namespace mars::utils; - + //access these strings only via toString(const NodeType&) and typeFromString(const std::string&). //synchronize this list with the enum NodeType in MARSDefs.h static const char* sTypeNames[NUMBER_OF_NODE_TYPES] = { @@ -149,9 +149,11 @@ namespace mars { { // handle node type if((it = config->find("physicmode")) != config->end()) { - std::string typeName = (std::string)it->second; + nodeType = trim(config->get("physicmode", nodeType)); try { - physicMode = typeFromString(trim(typeName)); + //Here we can check whether the node factory has this node type or not + //If not then load it dynamically (if available) + //else just throw an exception } catch(...) { //throw SceneParseException("invalid physicmode for node", -1); LOG_ERROR("could not get type for node: %s", name.c_str()); @@ -163,7 +165,7 @@ namespace mars { if(filename == "PRIMITIVE") { if(origName.empty()) { - origName = toString(physicMode); + origName = nodeType; } /* this is a valid case else if(origName != toString(physicMode)) { @@ -308,17 +310,15 @@ namespace mars { SET_VALUE("noPhysical", noPhysical, writeDefaults); SET_VALUE("movable", movable, writeDefaults); - if(writeDefaults || physicMode != defaultNode.physicMode) { + if(writeDefaults || nodeType != defaultNode.nodeType) { if(noPhysical) { try { - std::string tmp = toString(physicMode); - (*config)["physicmode"] = tmp; + (*config)["physicmode"] = nodeType; } catch (...) { } } - else { - std::string tmp = toString(physicMode); - (*config)["physicmode"] = tmp; + else {; + (*config)["physicmode"] = nodeType; } } diff --git a/interfaces/src/NodeData.h b/interfaces/src/NodeData.h index fa0f294ee..69e849b27 100644 --- a/interfaces/src/NodeData.h +++ b/interfaces/src/NodeData.h @@ -71,7 +71,7 @@ namespace mars { groupID = 0; index = 0; filename = "PRIMITIVE"; - physicMode = NODE_TYPE_UNDEFINED; + nodeType = ""; pos.setZero(); pivot.setZero(); rot.setIdentity(); @@ -133,14 +133,14 @@ namespace mars { * @param extents extents of the object * @param mass the mass of the object */ - void initPrimitive( NodeType type, const utils::Vector& extents, sReal mass ) + void initPrimitive(const std::string type, const utils::Vector& extents, sReal mass ) { - physicMode = type; + nodeType = type; ext = extents; this->mass = mass; filename = "PRIMITIVE"; - origName = toString(type); + origName = type; } bool fromConfigMap(configmaps::ConfigMap *config, std::string filenamePrefix, @@ -189,18 +189,11 @@ namespace mars { unsigned long index; /** - * This value holds the physical representation of a node. Valid values are: - * - NODE_TYPE_MESH - * - NODE_TYPE_BOX - * - NODE_TYPE_SPHERE - * - NODE_TYPE_CAPSULE - * - NODE_TYPE_CYLINDER - * - NODE_TYPE_PLANE - * - NODE_TYPE_TERRAIN - * . - * \verbatim Default value: NODE_TYPE_UNDEFINED \endverbatim + * @brief The node type name as string + * */ - NodeType physicMode; + + std::string nodeType; /** * The position of the node. @@ -249,7 +242,7 @@ namespace mars { /** * The size of the node. The ext vector is interpreted depending of the - * physicMode of the node: + * type of the node: * - mesh: ext defines the bounding box of the mesh * - box: ext defines the size in x, y, and z * - sphere: ext.x defines the radius @@ -266,7 +259,7 @@ namespace mars { /** * The mesh struct stores the poly information for a physical representation * of a mesh. This struct is automatically derived from the visual node - * by the simulation if the physicMode is set to NODE_TYPE_MESH. + * by the simulation if the type is set to mesh. * \verbatim Default value: see snmesh \endverbatim \sa snmesh */ snmesh mesh; @@ -307,7 +300,7 @@ namespace mars { /** - * If the physicMode is set to NODE_TYPE_TERRAIN, this pointer stores + * If the node type is set to terrain, this pointer stores * all information to create a physical representation of a height map. * \verbatim Default value: 0 \endverbatim \sa terrainStruct */ @@ -355,7 +348,7 @@ namespace mars { contact_params c_params; /** - * Generally the inertia of the physical body is calculated by the physicMode + * Generally the inertia of the physical body is calculated by the node type * and the mass of a node. In case of a mesh the inertia is calculated for * a box with the size given by NodeData::extent. If this boolean is set to * \c true, the inertia array (NodeData::inertia) is used instead. diff --git a/interfaces/src/sim/ControlCenter.h b/interfaces/src/sim/ControlCenter.h index d3d72bdc2..5f992aa43 100644 --- a/interfaces/src/sim/ControlCenter.h +++ b/interfaces/src/sim/ControlCenter.h @@ -65,6 +65,7 @@ namespace mars { namespace interfaces { class ControlCenter; + class StorageManagerInterface; class NodeManagerInterface; class JointManagerInterface; class MotorManagerInterface; @@ -84,6 +85,7 @@ namespace mars { ControlCenter(){ sim = NULL; cfg = NULL; + storage = NULL; nodes = NULL; joints = NULL; motors = NULL; @@ -95,6 +97,7 @@ namespace mars { } cfg_manager::CFGManagerInterface *cfg; + StorageManagerInterface *storage; NodeManagerInterface *nodes; JointManagerInterface *joints; MotorManagerInterface *motors; diff --git a/interfaces/src/sim/NodeManagerInterface.h b/interfaces/src/sim/NodeManagerInterface.h index ad03d1b49..e9d5a1fae 100644 --- a/interfaces/src/sim/NodeManagerInterface.h +++ b/interfaces/src/sim/NodeManagerInterface.h @@ -87,7 +87,7 @@ namespace mars { - virtual NodeId createPrimitiveNode(const std::string &name, NodeType type, + virtual NodeId createPrimitiveNode(const std::string &name, const std::string &type, bool movable=false, const utils::Vector &pos=utils::Vector::Zero(), const utils::Vector &extension=utils::Vector::Identity(), diff --git a/interfaces/src/sim/StorageManagerInterface.h b/interfaces/src/sim/StorageManagerInterface.h new file mode 100644 index 000000000..06ac8b73d --- /dev/null +++ b/interfaces/src/sim/StorageManagerInterface.h @@ -0,0 +1,54 @@ +/* + * Copyright 2011, 2012, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +/** + * \file StorageManagerInterface.h + * \author \n + * \brief + * + * \version + * \date + */ + +#ifndef STORAGE_MANAGER_INTERFACE_H +#define STORAGE_MANAGER_INTERFACE_H + +#ifdef _PRINT_HEADER_ + #warning "StorageManagerInterface.h" +#endif + + +namespace mars { + namespace interfaces { + + /** + * \author \n + * \brief + * + */ + class StorageManagerInterface { + public: + + }; + + } // end of namespace interfaces +} // end of namespace mars + +#endif // STORAGE_MANAGER_INTERFACE_H diff --git a/plugins/obstacle_generator/src/ObstacleGenerator.cpp b/plugins/obstacle_generator/src/ObstacleGenerator.cpp index 065b23440..f23cf1386 100644 --- a/plugins/obstacle_generator/src/ObstacleGenerator.cpp +++ b/plugins/obstacle_generator/src/ObstacleGenerator.cpp @@ -169,7 +169,7 @@ namespace mars { Vector boxsize(field_length, field_width, 1.0); Vector boxorientation(0, -params["incline_angle"], 0); NodeData box("incline", boxposition, eulerToQuaternion(boxorientation)); - box.initPrimitive(NODE_TYPE_BOX, boxsize, 1.0); + box.initPrimitive("box", boxsize, 1.0); box.material.texturename = textures["ground"]; if (textures["ground_bump"] != "") { box.material.bumpmap = textures["ground_bump"]; @@ -181,7 +181,7 @@ namespace mars { box.material.tex_scale = field_width/2.0; control->nodes->addNode(&box, false); oldNodeIDs.push_back(box.index); - // control->nodes->createPrimitiveNode("incline", NODE_TYPE_BOX, + // control->nodes->createPrimitiveNode("incline", "box", // false, boxposition, boxsize, 1.0, eulerToQuaternion(boxorientation), false)); platform_length = 2.0 + params["field_distance"]; platform_x_position = -1.0 + 0.5 * params["field_distance"]; @@ -195,7 +195,7 @@ namespace mars { Vector platformposition(platform_x_position, 0.0, -0.5 + params["ground_level"]); Vector platformorientation(0.0, 0.0, 0.0); NodeData platform("platform", platformposition, eulerToQuaternion(platformorientation)); - platform.initPrimitive(NODE_TYPE_BOX, platformsize, 1.0); + platform.initPrimitive("box", platformsize, 1.0); platform.material.texturename = textures["ground"]; if (textures["ground_bump"] != "") { platform.material.bumpmap = textures["ground_bump"]; @@ -315,10 +315,10 @@ namespace mars { } NodeData obstacle(name, position, orientation); if (bool_params["use_boxes"]) { - obstacle.initPrimitive(NODE_TYPE_BOX, size, 1.0); + obstacle.initPrimitive("box", size, 1.0); } else { - obstacle.initPrimitive(NODE_TYPE_CAPSULE, size, 1.0); + obstacle.initPrimitive("capsule", size, 1.0); } obstacle.material.texturename = textures["obstacle"]; obstacle.material.diffuseFront = Color(1.0, 1.0, 1.0, 1.0); diff --git a/sim/CMakeLists.txt b/sim/CMakeLists.txt index 770552876..6cf05f527 100644 --- a/sim/CMakeLists.txt +++ b/sim/CMakeLists.txt @@ -3,6 +3,8 @@ set(PROJECT_VERSION 1.0) set(PROJECT_DESCRIPTION "This library ...") cmake_minimum_required(VERSION 2.6) +set(CMAKE_BUILD_TYPE DEBUG) + find_package(Rock) include(FindPkgConfig) @@ -84,11 +86,30 @@ set(SOURCES_H src/core/Simulator.h src/sensors/RotatingRaySensor.h - src/physics/JointPhysics.h - src/physics/NodePhysics.h src/physics/WorldPhysics.h src/physics/ContactsPhysics.hpp + src/physics/objects/ODEObject.h + src/physics/objects/ODEObjectFactory.h + src/physics/objects/ObjectFactoryInterface.h + src/physics/objects/ODEBox.h + src/physics/objects/ODECapsule.h + src/physics/objects/ODECylinder.h + src/physics/objects/ODEHeightField.h + src/physics/objects/ODEMesh.h + src/physics/objects/ODEPlane.h + src/physics/objects/ODESphere.h + + src/physics/joints/ODEJoint.h + src/physics/joints/ODEJointFactory.h + src/physics/joints/JointFactoryInterface.h + src/physics/joints/ODEBallJoint.h + src/physics/joints/ODEFixedJoint.h + src/physics/joints/ODEHinge2Joint.h + src/physics/joints/ODEHingeJoint.h + src/physics/joints/ODESliderJoint.h + src/physics/joints/ODEUniversalJoint.h + src/sensors/CameraSensor.h src/sensors/IDListConfig.h src/sensors/Joint6DOFSensor.h @@ -136,8 +157,25 @@ set(TARGET_SRC src/sensors/MultiLevelLaserRangeFinder.cpp src/sensors/RotatingRaySensor.cpp - src/physics/JointPhysics.cpp - src/physics/NodePhysics.cpp + src/physics/objects/ODEObject.cpp + src/physics/objects/ODEObjectFactory.cpp + src/physics/objects/ODEBox.cpp + src/physics/objects/ODECapsule.cpp + src/physics/objects/ODECylinder.cpp + src/physics/objects/ODEHeightField.cpp + src/physics/objects/ODEMesh.cpp + src/physics/objects/ODEPlane.cpp + src/physics/objects/ODESphere.cpp + + src/physics/joints/ODEJoint.cpp + src/physics/joints/ODEJointFactory.cpp + src/physics/joints/ODEBallJoint.cpp + src/physics/joints/ODEFixedJoint.cpp + src/physics/joints/ODEHinge2Joint.cpp + src/physics/joints/ODEHingeJoint.cpp + src/physics/joints/ODESliderJoint.cpp + src/physics/joints/ODEUniversalJoint.cpp + src/physics/WorldPhysics.cpp src/sensors/CameraSensor.cpp diff --git a/sim/src/core/JointManager.cpp b/sim/src/core/JointManager.cpp index 8a207a7f8..d41209001 100644 --- a/sim/src/core/JointManager.cpp +++ b/sim/src/core/JointManager.cpp @@ -30,21 +30,28 @@ #include "SimJoint.h" #include "JointManager.h" #include "NodeManager.h" -#include "PhysicsMapper.h" - +#include "joints/ODEJointFactory.h" +#include "joints/ODEBallJoint.h" +#include "joints/ODEFixedJoint.h" +#include "joints/ODEHinge2Joint.h" +#include "joints/ODEHingeJoint.h" +#include "joints/ODESliderJoint.h" +#include "joints/ODEUniversalJoint.h" #include #include #include +#include #include #include #include #include #include + namespace mars { - namespace sim { +namespace sim { using namespace utils; using namespace interfaces; @@ -61,6 +68,13 @@ namespace mars { JointManager::JointManager(ControlCenter *c) { control = c; next_joint_id = 1; + + ODEJointFactory::Instance().addJointType("ball", &ODEBallJoint::instanciate); + ODEJointFactory::Instance().addJointType("fixed", &ODEFixedJoint::instanciate); + ODEJointFactory::Instance().addJointType("hinge2", &ODEHinge2Joint::instanciate); + ODEJointFactory::Instance().addJointType("hinge", &ODEHingeJoint::instanciate); + ODEJointFactory::Instance().addJointType("slider", &ODESliderJoint::instanciate); + ODEJointFactory::Instance().addJointType("universal", &ODEUniversalJoint::instanciate); } unsigned long JointManager::addJoint(JointData *jointS, bool reload) { @@ -84,8 +98,6 @@ namespace mars { return 0; } - // create an interface object to the physics - newJointInterface = PhysicsMapper::newJointPhysics(control->sim->getPhysics()); // reset the anchor //if node index is 0, the node connects to the environment. node1 = control->nodes->getSimNode(jointS->nodeIndex1); @@ -108,8 +120,19 @@ namespace mars { jointS->anchor = (node1->getPosition() + node2->getPosition()) / 2.; } + // create an interface object to the physics + newJointInterface = ODEJointFactory::Instance().createJoint(control->sim->getPhysics(), jointS, i_node1, i_node2); + // create the physical node data - if (newJointInterface->createJoint(jointS, i_node1, i_node2)) { + if (newJointInterface.get() == nullptr) { + LOG_ERROR("JointManager::addJoint: No joint was created in physics."); + // if no node was created in physics + // delete the objects + newJointInterface.reset(); + // and return false + return 0; + } + else { // put all data to the correct place iMutex.lock(); // set the next free id @@ -132,7 +155,7 @@ namespace mars { next_joint_id = des_id + 1; } } - std::shared_ptr newJoint = std::make_shared(control, *jointS); + std::shared_ptr newJoint = std::make_shared(control, *jointS); newJoint->setAttachedNodes(node1, node2); // newJoint->setSJoint(*jointS); newJoint->setPhysicalJoint(newJointInterface); @@ -140,13 +163,6 @@ namespace mars { iMutex.unlock(); control->sim->sceneHasChanged(false); return jointS->index; - } else { - std::cerr << "JointManager: Could not create new joint (JointInterface::createJoint() returned false)." << std::endl; - // if no node was created in physics - // delete the objects - newJointInterface.reset(); - // and return false - return 0; } } @@ -587,5 +603,5 @@ namespace mars { } } - } // end of namespace sim +} // end of namespace sim } // end of namespace mars diff --git a/sim/src/core/NodeManager.cpp b/sim/src/core/NodeManager.cpp index 03b164c29..69fe12607 100644 --- a/sim/src/core/NodeManager.cpp +++ b/sim/src/core/NodeManager.cpp @@ -29,7 +29,14 @@ #include "SimJoint.h" #include "NodeManager.h" #include "JointManager.h" -#include "PhysicsMapper.h" +#include "objects/ODEObjectFactory.h" +#include "objects/ODEBox.h" +#include "objects/ODECapsule.h" +#include "objects/ODECylinder.h" +#include "objects/ODEHeightField.h" +#include "objects/ODEMesh.h" +#include "objects/ODEPlane.h" +#include "objects/ODESphere.h" #include #include @@ -75,11 +82,21 @@ namespace mars { GraphicsUpdateInterface *gui = static_cast(this); control->graphics->addGraphicsUpdateInterface(gui); } + ODEObjectFactory::Instance().addObjectType("box", &ODEBox::instanciate); + ODEObjectFactory::Instance().addObjectType("capsule", &ODECapsule::instanciate); + ODEObjectFactory::Instance().addObjectType("cylinder", &ODECylinder::instanciate); + ODEObjectFactory::Instance().addObjectType("terrain", &ODEHeightField::instanciate); + ODEObjectFactory::Instance().addObjectType("mesh", &ODEMesh::instanciate); + ODEObjectFactory::Instance().addObjectType("plane", &ODEPlane::instanciate); + ODEObjectFactory::Instance().addObjectType("sphere", &ODESphere::instanciate); } + NodeManager::~NodeManager() + { + } NodeId NodeManager::createPrimitiveNode(const std::string &name, - NodeType type, + const std::string &type, bool moveable, const Vector &pos, const Vector &extension, @@ -115,7 +132,7 @@ namespace mars { if (!reload) { iMutex.lock(); NodeData reloadNode = *nodeS; - if((nodeS->physicMode == NODE_TYPE_TERRAIN) && nodeS->terrain ) { + if((nodeS->nodeType == "terrain") && nodeS->terrain ) { if(!control->loadCenter) { LOG_ERROR("NodeManager:: loadCenter is missing, can not create Node"); iMutex.unlock(); @@ -166,7 +183,7 @@ namespace mars { } // convert obj to ode mesh - if((nodeS->physicMode == NODE_TYPE_MESH) && (nodeS->terrain == 0) ) { + if((nodeS->nodeType == "mesh") && (nodeS->terrain == 0) ) { if(!control->loadCenter) { LOG_ERROR("NodeManager:: loadCenter is missing, can not create Node"); return INVALID_ID; @@ -186,7 +203,7 @@ namespace mars { } control->loadCenter->loadMesh->getPhysicsFromMesh(nodeS); } - if((nodeS->physicMode == NODE_TYPE_TERRAIN) && nodeS->terrain ) { + if((nodeS->nodeType == "terrain") && nodeS->terrain ) { if(!nodeS->terrain->pixelData) { if(!control->loadCenter) { LOG_ERROR("NodeManager:: loadCenter is missing, can not create Node"); @@ -243,8 +260,9 @@ namespace mars { // create the physical node data if(! (nodeS->noPhysical)){ // create an interface object to the physics - std::shared_ptr newNodeInterface = PhysicsMapper::newNodePhysics(control->sim->getPhysics()); - if (!newNodeInterface->createNode(nodeS)) { + std::shared_ptr newNodeInterface = ODEObjectFactory::Instance().createObject(control->sim->getPhysics(), nodeS); + + if (newNodeInterface.get() == nullptr) { // if no node was created in physics // delete the objects newNode.reset(); @@ -253,6 +271,7 @@ namespace mars { LOG_ERROR("NodeManager::addNode: No node was created in physics."); return INVALID_ID; } + // put all data to the correct place // newNode->setSNode(*nodeS); newNode->setInterface(newNodeInterface); @@ -289,14 +308,12 @@ namespace mars { physicalRep.visual_offset_rot = Quaternion::Identity(); physicalRep.visual_size = Vector(0.0, 0.0, 0.0); physicalRep.map["sharedDrawID"] = 0lu; - physicalRep.map["visualType"] = NodeData::toString(nodeS->physicMode); - if(nodeS->physicMode != NODE_TYPE_TERRAIN) { - if(nodeS->physicMode != NODE_TYPE_MESH) { + physicalRep.map["visualType"] = nodeS->nodeType; + if(nodeS->nodeType != "terrain") { + if(nodeS->nodeType != "mesh") { physicalRep.filename = "PRIMITIVE"; //physicalRep.filename = nodeS->filename; - if(nodeS->physicMode > 0 && nodeS->physicMode < NUMBER_OF_NODE_TYPES){ - physicalRep.origName = NodeData::toString(nodeS->physicMode); - } + physicalRep.origName = nodeS->nodeType; } if(loadGraphics) { id = control->graphics->addDrawObject(physicalRep, @@ -367,7 +384,7 @@ namespace mars { newNode.movable = false; newNode.groupID = 0; newNode.relative_id = 0; - newNode.physicMode = NODE_TYPE_TERRAIN; + newNode.nodeType = "terrain"; newNode.pos = Vector(0.0, 0.0, 0.0); newNode.rot = eulerToQuaternion(trot); newNode.density = 1; @@ -1981,7 +1998,7 @@ namespace mars { Vector scale; if(sNode.filename == "PRIMITIVE") { scale = nodeS->ext; - if(sNode.physicMode == NODE_TYPE_SPHERE) { + if(sNode.nodeType == "sphere") { scale.x() *= 2; scale.y() = scale.z() = scale.x(); } diff --git a/sim/src/core/NodeManager.h b/sim/src/core/NodeManager.h index c50d0f5c7..7acf31e4e 100644 --- a/sim/src/core/NodeManager.h +++ b/sim/src/core/NodeManager.h @@ -54,10 +54,9 @@ namespace mars { public: NodeManager(interfaces::ControlCenter *c, lib_manager::LibManager *theManager); - virtual ~NodeManager(){} - + virtual ~NodeManager(); virtual interfaces::NodeId createPrimitiveNode(const std::string &name, - interfaces::NodeType type, + const std::string &type, bool movable=false, const utils::Vector &pos=utils::Vector::Zero(), const utils::Vector &extension=utils::Vector::Identity(), @@ -165,6 +164,7 @@ namespace mars { const std::string &value); private: + interfaces::NodeId next_node_id; bool update_all_nodes; int visual_rep; diff --git a/sim/src/core/PhysicsMapper.cpp b/sim/src/core/PhysicsMapper.cpp index 3ce215069..80f9533ba 100644 --- a/sim/src/core/PhysicsMapper.cpp +++ b/sim/src/core/PhysicsMapper.cpp @@ -29,25 +29,14 @@ #include "PhysicsMapper.h" namespace mars { - namespace sim { +namespace sim { using namespace mars::interfaces; - std::shared_ptr PhysicsMapper::newWorldPhysics(ControlCenter *control) { + std::shared_ptr PhysicsMapper::newWorldPhysics(ControlCenter *control) { std::shared_ptr worldPhysics = std::make_shared(control); return std::static_pointer_cast(worldPhysics); - } + } - std::shared_ptr PhysicsMapper::newNodePhysics(std::shared_ptr worldPhysics) { - // Create a nodePhysics with the worldPhysics as constructor parameter, then downcast to a Node interface and return - std::shared_ptr nodePhysics = std::make_shared(worldPhysics); - return std::static_pointer_cast(nodePhysics); - } - - std::shared_ptr PhysicsMapper::newJointPhysics(std::shared_ptr worldPhysics) { - std::shared_ptr jointPhysics = std::make_shared(worldPhysics); - return std::static_pointer_cast(jointPhysics); - } - - } // end of namespace sim +} // end of namespace sim } // end of namespace mars diff --git a/sim/src/core/PhysicsMapper.h b/sim/src/core/PhysicsMapper.h index 0ef303d34..6353b9f5f 100644 --- a/sim/src/core/PhysicsMapper.h +++ b/sim/src/core/PhysicsMapper.h @@ -34,8 +34,6 @@ #endif #include "WorldPhysics.h" -#include "NodePhysics.h" -#include "JointPhysics.h" namespace mars { namespace sim { @@ -43,9 +41,6 @@ namespace mars { class PhysicsMapper { public: static std::shared_ptr newWorldPhysics(interfaces::ControlCenter *control); - static std::shared_ptr newNodePhysics(std::shared_ptr worldPhysics); - static std::shared_ptr newJointPhysics(std::shared_ptr worldPhysics); - }; } // end of namespace sim diff --git a/sim/src/core/SimNode.cpp b/sim/src/core/SimNode.cpp index 8ebd12e28..244954a25 100644 --- a/sim/src/core/SimNode.cpp +++ b/sim/src/core/SimNode.cpp @@ -373,14 +373,14 @@ namespace mars { return sNode.mesh; } - void SimNode::setPhysicMode(NodeType mode) { + void SimNode::setNodeType(const std::string &type) { MutexLocker locker(&iMutex); - sNode.physicMode = mode; + sNode.nodeType = type; } - NodeType SimNode::getPhysicMode() const { + const std::string SimNode::getNodeType() const { MutexLocker locker(&iMutex); - return sNode.physicMode; + return sNode.nodeType; } void SimNode::setMovable(bool movable) { @@ -472,7 +472,7 @@ namespace mars { this->sNode.origName = sNode.origName.c_str(); this->sNode.filename = sNode.filename.c_str(); this->sNode.groupID = sNode.groupID; - this->sNode.physicMode = sNode.physicMode; + this->sNode.nodeType = sNode.nodeType; this->sNode.movable = sNode.movable; this->sNode.noPhysical = sNode.noPhysical; this->sNode.density = sNode.density; diff --git a/sim/src/core/SimNode.h b/sim/src/core/SimNode.h index 284f2854f..7d392ef28 100644 --- a/sim/src/core/SimNode.h +++ b/sim/src/core/SimNode.h @@ -96,7 +96,7 @@ namespace mars { interfaces::sReal getMass(void) const; ///< Returns the mass of the node. const interfaces::snmesh getMesh(void) const; ///< Returns the mesh of the node. const std::string getName(void) const; ///< - interfaces::NodeType getPhysicMode(void) const; + const std::string getNodeType(void) const; const utils::Vector getPosition(void) const; const utils::Vector getVisualPosition(void) const; const utils::Quaternion getRotation(void) const; ///< Returns the rotation of the node. @@ -139,7 +139,7 @@ namespace mars { void setGraphicsID(unsigned long g_id); unsigned long getGraphicsID(void) const; void setGraphicsID2(unsigned long g_id); - void setPhysicMode(interfaces::NodeType mode); ///< Sets the physic mode of the node. See getPhysicMode for list of modes. + void setNodeType(const std::string &type); ///< Sets the physic mode of the node. See NodeManager for list of types. const utils::Vector setPosition(const utils::Vector &newPosition, bool move_group); ///< Sets the position of the node. void setPositionOffset(const utils::Vector &offset); const utils::Quaternion setRotation(const utils::Quaternion &rotation, bool move_all); ///< Sets the rotation of the node. diff --git a/sim/src/core/Simulator.cpp b/sim/src/core/Simulator.cpp index 698349a89..d12575560 100644 --- a/sim/src/core/Simulator.cpp +++ b/sim/src/core/Simulator.cpp @@ -288,6 +288,11 @@ namespace mars { } // If any manager is missing, create it from the default type. + //if (control->storage == NULL) { + // LOG_ERROR("[Simulator] No storage manager is defined."); + // throw std::exception(); + //} + if (control->nodes == NULL) { LOG_DEBUG("[Simulator] Set default node manager in control->nodes"); control->nodes = new NodeManager(control, libManager); diff --git a/sim/src/physics/WorldPhysics.cpp b/sim/src/physics/WorldPhysics.cpp index c3fd480a6..1f2347d5c 100644 --- a/sim/src/physics/WorldPhysics.cpp +++ b/sim/src/physics/WorldPhysics.cpp @@ -37,7 +37,7 @@ */ #include "WorldPhysics.h" -#include "NodePhysics.h" +#include "objects/ODEObject.h" #include "SimNode.h" @@ -354,7 +354,7 @@ namespace mars { std::vector nodeIds = control->nodes->getNodeIDs(nodeName); std::shared_ptr nodePtr = control->nodes->getSimNode(nodeIds[0]); std::shared_ptr nodeIfPtr = nodePtr->getInterface(); - std::shared_ptr nodePhysPtr = std::dynamic_pointer_cast(nodeIfPtr); + std::shared_ptr nodePhysPtr = std::dynamic_pointer_cast(nodeIfPtr); std::vector contactFeedbacks = nodePhysPtr->addContacts(colContacts, world, contactgroup); std::vector * contactPoints = new std::vector(); @@ -499,7 +499,7 @@ namespace mars { * - retruned true if a body was created, otherwise retruned false */ bool WorldPhysics::getCompositeBody(int comp_group, dBodyID* body, - NodePhysics *node) { + ODEObject *node) { body_nbr_tupel tmp_tupel; // if comp_group is bad, debug something @@ -538,9 +538,9 @@ namespace mars { * counter of connected geoms * - otherwise, if only one geom is connected to the body, destroy the body */ - void WorldPhysics::destroyBody(dBodyID theBody, NodePhysics* node) { + void WorldPhysics::destroyBody(dBodyID theBody, ODEObject* node) { std::vector::iterator iter; - std::vector::iterator jter; + std::vector::iterator jter; for(iter = comp_body_list.begin(); iter != comp_body_list.end(); iter++) { if((*iter).body == theBody) { @@ -1017,7 +1017,7 @@ namespace mars { */ void WorldPhysics::resetCompositeMass(dBodyID theBody) { std::vector::iterator iter; - std::vector::iterator jter; + std::vector::iterator jter; dMass bodyMass, tmpMass; //bool first = 1; @@ -1037,7 +1037,7 @@ namespace mars { void WorldPhysics::moveCompositeMassCenter(dBodyID theBody, dReal x, dReal y, dReal z) { std::vector::iterator iter; - std::vector::iterator jter; + std::vector::iterator jter; const dReal *bpos; // first we have to calculate the offset in bodyframe @@ -1064,7 +1064,7 @@ namespace mars { dMassSetZero(&sumMass); for(iter = nodes.begin(); iter != nodes.end(); iter++) { - (std::static_pointer_cast(*iter))->getAbsMass(&tMass); + (std::static_pointer_cast(*iter))->getAbsMass(&tMass); dMassAdd(&sumMass, &tMass); } center.x() = sumMass.c[0]; diff --git a/sim/src/physics/WorldPhysics.h b/sim/src/physics/WorldPhysics.h index 1a02830ba..7a837566f 100644 --- a/sim/src/physics/WorldPhysics.h +++ b/sim/src/physics/WorldPhysics.h @@ -52,7 +52,7 @@ namespace mars { namespace sim { - class NodePhysics; + class ODEObject; /** * The struct is used to handle some sensors in the physical @@ -78,7 +78,7 @@ namespace mars { dBodyID body; int comp_group; unsigned int connected_geoms; - std::vector comp_nodes; + std::vector comp_nodes; }; /** @@ -106,8 +106,8 @@ namespace mars { // this functions are used by the other physical classes dWorldID getWorld(void) const; dSpaceID getSpace(void) const; - bool getCompositeBody(int comp_group, dBodyID *body, NodePhysics *node); - void destroyBody(dBodyID theBody, NodePhysics *node); + bool getCompositeBody(int comp_group, dBodyID *body, ODEObject *node); + void destroyBody(dBodyID theBody, ODEObject *node); dReal getWorldStep(void); void resetCompositeMass(dBodyID theBody); void moveCompositeMassCenter(dBodyID theBody, dReal x, dReal y, dReal z); diff --git a/sim/src/physics/joints/JointFactoryInterface.h b/sim/src/physics/joints/JointFactoryInterface.h new file mode 100644 index 000000000..6186155e8 --- /dev/null +++ b/sim/src/physics/joints/JointFactoryInterface.h @@ -0,0 +1,45 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file JointFactoryInterface.h + * \author Muhammad Haider Khan Lodhi + * \brief "ObjectFactoryInterface" is an interface to create physical ode objects for the nodes. + * + */ + +#pragma once +#include "WorldPhysics.h" +#include + +namespace mars{ +namespace sim{ + +using namespace ::mars::interfaces; + +class JointFactoryInterface{ +public: + virtual std::shared_ptr createJoint(std::shared_ptr worldPhysics, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2) = 0; +}; +} +} \ No newline at end of file diff --git a/sim/src/physics/joints/ODEBallJoint.cpp b/sim/src/physics/joints/ODEBallJoint.cpp new file mode 100644 index 000000000..f14c1a77d --- /dev/null +++ b/sim/src/physics/joints/ODEBallJoint.cpp @@ -0,0 +1,90 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODEBallJoint.h" + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEBallJoint::ODEBallJoint(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2) + : ODEJoint(world) { + createJoint(joint, node1, node2 ); + } + + ODEBallJoint::~ODEBallJoint(void) { + } + + ODEJoint* ODEBallJoint::instanciate(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2){ + return new ODEBallJoint(world, joint, node1, node2); + } + + bool ODEBallJoint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2){ + + jointId= dJointCreateBall(theWorld->getWorld(),0); + dJointAttach(jointId, body1, body2); + dJointSetBallAnchor(jointId, jointS->anchor.x(), jointS->anchor.y(), + jointS->anchor.z()); + dJointSetBallParam(jointId, dParamCFM, cfm1); + dJointSetBallParam(jointId, dParamERP, erp1); + /* + ball_motor = dJointCreateAMotor(theWorld->getWorld(), 0); + dJointAttach(ball_motor, body1, body2); + dJointSetAMotorNumAxes(ball_motor, 3); + dJointSetAMotorAxis(ball_motor, 0, 1, 1,0,0); + dJointSetAMotorAxis(ball_motor, 2, 2, 0,1,0); + + if(damping > 0.00000001) { + dJointSetAMotorParam(ball_motor, dParamVel, 0); + dJointSetAMotorParam(ball_motor, dParamVel2, 0); + dJointSetAMotorParam(ball_motor, dParamVel3, 0); + dJointSetAMotorParam(ball_motor, dParamFMax, damping); + dJointSetAMotorParam(ball_motor, dParamFMax2, damping); + dJointSetAMotorParam(ball_motor, dParamFMax3, damping); + } + else if(spring > 0.00000001) { + dJointSetAMotorParam(ball_motor, dParamLoStop, lo2); + dJointSetAMotorParam(ball_motor, dParamLoStop2, lo2); + dJointSetAMotorParam(ball_motor, dParamLoStop3, lo2); + dJointSetAMotorParam(ball_motor, dParamHiStop, hi1); + dJointSetAMotorParam(ball_motor, dParamHiStop2, hi2); + dJointSetAMotorParam(ball_motor, dParamHiStop3, hi2); + dJointSetAMotorParam(ball_motor, dParamCFM, cfm1); + dJointSetAMotorParam(ball_motor, dParamCFM2, cfm2); + dJointSetAMotorParam(ball_motor, dParamCFM3, cfm2); + dJointSetAMotorParam(ball_motor, dParamERP, erp1); + dJointSetAMotorParam(ball_motor, dParamERP2, erp2); + dJointSetAMotorParam(ball_motor, dParamERP3, erp2); + } + dJointSetAMotorMode (ball_motor, dAMotorEuler); + */ + return 1; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/joints/ODEBallJoint.h b/sim/src/physics/joints/ODEBallJoint.h new file mode 100644 index 000000000..6b46b3ae3 --- /dev/null +++ b/sim/src/physics/joints/ODEBallJoint.h @@ -0,0 +1,63 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODEBallJoint.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_BALL_JOINT_H +#define ODE_BALL_JOINT_H + +#ifdef _PRINT_HEADER_ + #warning "ODEBallJoint.h" +#endif + +#include +#include "ODEJoint.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODEBallJoint : public ODEJoint { + public: + ODEBallJoint(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + virtual ~ODEBallJoint(void); + static ODEJoint* instanciate(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_BALL_JOINT_H diff --git a/sim/src/physics/joints/ODEFixedJoint.cpp b/sim/src/physics/joints/ODEFixedJoint.cpp new file mode 100644 index 000000000..d8e976bf1 --- /dev/null +++ b/sim/src/physics/joints/ODEFixedJoint.cpp @@ -0,0 +1,65 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODEFixedJoint.h" + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEFixedJoint::ODEFixedJoint(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2) + : ODEJoint(world) { + createJoint(joint, node1, node2 ); + } + + ODEFixedJoint::~ODEFixedJoint(void) { + } + + ODEJoint* ODEFixedJoint::instanciate(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2){ + return new ODEFixedJoint(world, joint, node1, node2); + } + + bool ODEFixedJoint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2){ + if (body1 || body2){ + CPP_UNUSED(jointS); + jointId = dJointCreateFixed(theWorld->getWorld(), 0); + dJointAttach(jointId, body1, body2); + dJointSetFixed(jointId); + // used for the integration study of the SpaceClimber + //dJointSetFixedParam(jointId, dParamCFM, cfm1);//0.0002); + //dJointSetFixedParam(jointId, dParamERP, erp1);//0.0002); + //dJointSetFixedParam(jointId, dParamCFM, 0.001); + } + else { + return 0; + } + return 1; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/joints/ODEFixedJoint.h b/sim/src/physics/joints/ODEFixedJoint.h new file mode 100644 index 000000000..2c3f9ce30 --- /dev/null +++ b/sim/src/physics/joints/ODEFixedJoint.h @@ -0,0 +1,63 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODEFixedJoint.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_FIXED_JOINT_H +#define ODE_FIXED_JOINT_H + +#ifdef _PRINT_HEADER_ + #warning "ODEFixedJoint.h" +#endif + +#include +#include "ODEJoint.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODEFixedJoint : public ODEJoint { + public: + ODEFixedJoint(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + virtual ~ODEFixedJoint(void); + static ODEJoint* instanciate(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_FIXED_JOINT_H diff --git a/sim/src/physics/joints/ODEHinge2Joint.cpp b/sim/src/physics/joints/ODEHinge2Joint.cpp new file mode 100644 index 000000000..7a7006a41 --- /dev/null +++ b/sim/src/physics/joints/ODEHinge2Joint.cpp @@ -0,0 +1,88 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODEHinge2Joint.h" + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEHinge2Joint::ODEHinge2Joint(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2) + : ODEJoint(world) { + createJoint(joint, node1, node2 ); + } + + ODEHinge2Joint::~ODEHinge2Joint(void) { + } + + ODEJoint* ODEHinge2Joint::instanciate(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2){ + return new ODEHinge2Joint(world, joint, node1, node2); + } + + bool ODEHinge2Joint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2){ + + if (body1 || body2){ + + jointId= dJointCreateHinge2(theWorld->getWorld(),0); + dJointAttach(jointId, body1, body2); + dJointSetHinge2Anchor(jointId, jointS->anchor.x(), jointS->anchor.y(), + jointS->anchor.z()); + dJointSetHinge2Axis1(jointId, jointS->axis1.x(), jointS->axis1.y(), + jointS->axis1.z()); + dJointSetHinge2Axis2(jointId, jointS->axis2.x(), jointS->axis2.y(), + jointS->axis2.z()); + + if(damping>0.00000001) { + dJointSetHinge2Param(jointId, dParamFMax, damping); + dJointSetHinge2Param(jointId, dParamVel, 0); + dJointSetHinge2Param(jointId, dParamFMax2, damping); + dJointSetHinge2Param(jointId, dParamVel2, 0); + } + if(spring > 0.00000001) { + dJointSetHinge2Param(jointId, dParamLoStop, lo1); + dJointSetHinge2Param(jointId, dParamHiStop, hi1); + dJointSetHinge2Param(jointId, dParamLoStop2, lo2); + dJointSetHinge2Param(jointId, dParamHiStop2, hi2); + dJointSetHinge2Param(jointId, dParamStopCFM, cfm1); + dJointSetHinge2Param(jointId, dParamStopERP, erp1); + dJointSetHinge2Param(jointId, dParamStopCFM2, cfm2); + dJointSetHinge2Param(jointId, dParamStopERP2, erp2); + dJointSetHinge2Param(jointId, dParamCFM, cfm); + } + if(jointCFM > 0) { + dJointSetHinge2Param(jointId, dParamCFM, jointCFM); + } + } + else { + return 0; + } + return 1; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/joints/ODEHinge2Joint.h b/sim/src/physics/joints/ODEHinge2Joint.h new file mode 100644 index 000000000..4bfe0dfab --- /dev/null +++ b/sim/src/physics/joints/ODEHinge2Joint.h @@ -0,0 +1,63 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODEHinge2Joint.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_HINGE2_JOINT_H +#define ODE_HINGE2_JOINT_H + +#ifdef _PRINT_HEADER_ + #warning "ODEHinge2Joint.h" +#endif + +#include +#include "ODEJoint.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODEHinge2Joint : public ODEJoint { + public: + ODEHinge2Joint(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + virtual ~ODEHinge2Joint(void); + static ODEJoint* instanciate(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_HINGE2_JOINT_H diff --git a/sim/src/physics/joints/ODEHingeJoint.cpp b/sim/src/physics/joints/ODEHingeJoint.cpp new file mode 100644 index 000000000..68db6b5c2 --- /dev/null +++ b/sim/src/physics/joints/ODEHingeJoint.cpp @@ -0,0 +1,84 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODEHingeJoint.h" + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEHingeJoint::ODEHingeJoint(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2) + : ODEJoint(world) { + createJoint(joint, node1, node2 ); + } + + ODEHingeJoint::~ODEHingeJoint(void) { + } + + ODEJoint* ODEHingeJoint::instanciate(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2){ + return new ODEHingeJoint(world, joint, node1, node2); + } + + bool ODEHingeJoint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2){ + + jointId= dJointCreateHinge(theWorld->getWorld(),0); + dJointAttach(jointId, body1, body2); + dJointSetHingeAnchor(jointId, jointS->anchor.x(), jointS->anchor.y(), + jointS->anchor.z()); + dJointSetHingeAxis(jointId, jointS->axis1.x(), jointS->axis1.y(), + jointS->axis1.z()); + + if(damping>0.00000001) { + dJointSetHingeParam(jointId, dParamFMax, damping); + dJointSetHingeParam(jointId, dParamVel, 0); + } + if(spring > 0.00000001) { + dJointSetHingeParam(jointId, dParamLoStop, lo1); + dJointSetHingeParam(jointId, dParamHiStop, hi1); + dJointSetHingeParam(jointId, dParamStopCFM, cfm1); + dJointSetHingeParam(jointId, dParamStopERP, erp1); + //dJointSetHingeParam(jointId, dParamCFM, cfm); + //dJointSetHingeParam(jointId, dParamFudgeFactor, 1); + } + else if(lo1 != 0) { + dJointSetHingeParam(jointId, dParamLoStop, lo1); + dJointSetHingeParam(jointId, dParamHiStop, hi1); + } + if(jointCFM > 0) { + dJointSetHingeParam(jointId, dParamCFM, jointCFM); + } + // good value for the SpaceClimber robot + //dJointSetHingeParam(jointId, dParamCFM, 0.03); + //dJointSetHingeParam(jointId, dParamCFM, 0.018); + //dJointSetHingeParam(jointId, dParamCFM, 0.09); + + return 1; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/joints/ODEHingeJoint.h b/sim/src/physics/joints/ODEHingeJoint.h new file mode 100644 index 000000000..da2a03eac --- /dev/null +++ b/sim/src/physics/joints/ODEHingeJoint.h @@ -0,0 +1,63 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODEHingeJoint.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_HINGE_JOINT_H +#define ODE_HINGE_JOINT_H + +#ifdef _PRINT_HEADER_ + #warning "ODEHingeJoint.h" +#endif + +#include +#include "ODEJoint.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODEHingeJoint : public ODEJoint { + public: + ODEHingeJoint(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + virtual ~ODEHingeJoint(void); + static ODEJoint* instanciate(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_HINGE_JOINT_H diff --git a/sim/src/physics/JointPhysics.cpp b/sim/src/physics/joints/ODEJoint.cpp similarity index 70% rename from sim/src/physics/JointPhysics.cpp rename to sim/src/physics/joints/ODEJoint.cpp index 7222900c5..0953d646c 100644 --- a/sim/src/physics/JointPhysics.cpp +++ b/sim/src/physics/joints/ODEJoint.cpp @@ -19,24 +19,24 @@ */ /* - * \file JointPhysics.h + * \file ODEJoint.h * \autor Larbi Abdenebaoui - * \brief "JointPhysics" declares the physics for the joint using ode + * \brief "ODEJoint" declares the physics for the joint using ode * - * core "SimJoint" and the physics "JointPhysics" - * SimJoint ------> JointInterface(i_Joint) ------------> JointPhysics + * core "SimJoint" and the physics "ODEJoint" + * SimJoint ------> JointInterface(i_Joint) ------------> ODEJoint * */ #include -#include "JointPhysics.h" -#include "NodePhysics.h" +#include "ODEJoint.h" +#include "objects/ODEObject.h" #include namespace mars { - namespace sim { +namespace sim { using namespace utils; using namespace interfaces; @@ -45,7 +45,7 @@ namespace mars { * \brief the constructor of the Joint physics * initialize the attributes of the object */ - JointPhysics::JointPhysics(std::shared_ptr world){ + ODEJoint::ODEJoint(std::shared_ptr world){ theWorld = std::static_pointer_cast(world); jointId = ball_motor = 0; jointCFM = 0.0; @@ -55,6 +55,7 @@ namespace mars { spring = 0; body1 = 0; body2 = 0; + joint_created = false; } /** @@ -66,7 +67,7 @@ namespace mars { * post: * - all physical representation of the joint should be cleared */ - JointPhysics::~JointPhysics(void) { + ODEJoint::~ODEJoint(void) { MutexLocker locker(&(theWorld->iMutex)); if (jointId) { dJointDestroy(jointId); @@ -74,7 +75,7 @@ namespace mars { } - void JointPhysics::calculateCfmErp(const JointData *jointS) { + void ODEJoint::calculateCfmErp(const JointData *jointS) { // how to set erp and cfm // ERP = h kp / (h kp + kd) // CFM = 1 / (h kp + kd) @@ -103,11 +104,15 @@ namespace mars { cfm2 = (cfm2>0)?1/cfm2:0.000000000001; } + bool ODEJoint::isJointCreated(){ + return joint_created; + } + /** * \brief create the joint with the informations giving from jointS * */ - bool JointPhysics::createJoint(JointData *jointS, const std::shared_ptr node1, + bool ODEJoint::createJoint(interfaces::JointData *jointS, const std::shared_ptr node1, const std::shared_ptr node2) { #ifdef _VERIFY_WORLD_ fprintf(stderr, "joint %d ; %d ; %d ; %.4f, %.4f, %.4f ; %.4f, %.4f, %.4f\n", @@ -119,8 +124,8 @@ namespace mars { if ( theWorld && theWorld->existsWorld() ) { //get the bodies from the interfaces nodes //here we have to make some verifications - const std::shared_ptr n1 = std::dynamic_pointer_cast(node1); - const std::shared_ptr n2 = std::dynamic_pointer_cast(node2); + const std::shared_ptr n1 = std::dynamic_pointer_cast(node1); + const std::shared_ptr n2 = std::dynamic_pointer_cast(node2); dBodyID b1 = 0, b2 = 0; calculateCfmErp(jointS); @@ -133,46 +138,32 @@ namespace mars { if(n2) b2 = n2->getBody(); body1 = b1; body2 = b2; - switch(jointS->type) { - case JOINT_TYPE_HINGE: - createHinge(jointS, b1, b2); - break; - case JOINT_TYPE_HINGE2: - if(b1 || b2) createHinge2(jointS, b1, b2); - else return 0; - break; - case JOINT_TYPE_SLIDER: - createSlider(jointS, b1, b2); - break; - case JOINT_TYPE_BALL: - createBall(jointS, b1, b2); - break; - case JOINT_TYPE_UNIVERSAL: - if(b1 || b2) createUniversal(jointS, b1, b2); - else return 0; - break; - case JOINT_TYPE_FIXED: - if(b1 || b2) createFixed(jointS, b1, b2); - else return 0; - break; - default: - // Invalid type. No physically node will be created. - std::cerr << __FILE__":" << __LINE__<< " invalid joint type for joint \"" - << jointS->name << "\". No joint created." << std::endl; + + bool ret; + ret = createODEJoint(jointS, body1, body2); + if(ret == 0) { + // Error createing the joint return 0; - break; } + // we have created a joint. To get the information // of the forces the joint attached to the bodies // we need to set a feedback pointer for the joint (ode stuff) dJointSetFeedback(jointId, &feedback); + joint_created = true; return 1; } return 0; } + bool ODEJoint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2){ + std::cout << "ODEJoint: using default createODEJoint func. Did you forget to override it?." << std::endl; + LOG_WARN("ODEObject: using default createODEJoint func. Did you forget to override it?."); + return 0; + } + ///get the anchor of the joint - void JointPhysics::getAnchor(Vector* anchor) const { + void ODEJoint::getAnchor(Vector* anchor) const { dReal pos[4] = {0,0,0,0}; MutexLocker locker(&(theWorld->iMutex)); @@ -202,7 +193,7 @@ namespace mars { } // the next force and velocity methods are only in a beta state - void JointPhysics::setForceLimit(sReal max_force) { + void ODEJoint::setForceLimit(sReal max_force) { MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { @@ -221,7 +212,7 @@ namespace mars { } } - void JointPhysics::setForceLimit2(sReal max_force) { + void ODEJoint::setForceLimit2(sReal max_force) { MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { @@ -238,7 +229,7 @@ namespace mars { } } - void JointPhysics::setVelocity(sReal velocity) { + void ODEJoint::setVelocity(sReal velocity) { MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { @@ -257,7 +248,7 @@ namespace mars { } } - void JointPhysics::setVelocity2(sReal velocity) { + void ODEJoint::setVelocity2(sReal velocity) { MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { @@ -274,7 +265,7 @@ namespace mars { } } - sReal JointPhysics::getPosition(void) const { + sReal ODEJoint::getPosition(void) const { MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { @@ -294,7 +285,7 @@ namespace mars { return 0; } - sReal JointPhysics::getPosition2(void) const { + sReal ODEJoint::getPosition2(void) const { MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { @@ -305,219 +296,8 @@ namespace mars { return 0; } - - - /** - * \brief Creates a hinge joint in the physical environment - * - * We get a problem here. There two different way how a hinge joint - * can be used. First is can be used only as joint. So we should set - * the low and height stop to zero and use the stopcfm and stoperp values - * to simulate the spring-damping properties of the joint. The second case - * is, to use the hinge to simulate a motor. I don't know how the ode - * behavior will look like, if we set the same parameter and use the - * force and velocity params to simulate a motor. - * - */ - void JointPhysics::createHinge(JointData *jointS, dBodyID body1, - dBodyID body2){ - - jointId= dJointCreateHinge(theWorld->getWorld(),0); - dJointAttach(jointId, body1, body2); - dJointSetHingeAnchor(jointId, jointS->anchor.x(), jointS->anchor.y(), - jointS->anchor.z()); - dJointSetHingeAxis(jointId, jointS->axis1.x(), jointS->axis1.y(), - jointS->axis1.z()); - - if(damping>0.00000001) { - dJointSetHingeParam(jointId, dParamFMax, damping); - dJointSetHingeParam(jointId, dParamVel, 0); - } - if(spring > 0.00000001) { - dJointSetHingeParam(jointId, dParamLoStop, lo1); - dJointSetHingeParam(jointId, dParamHiStop, hi1); - dJointSetHingeParam(jointId, dParamStopCFM, cfm1); - dJointSetHingeParam(jointId, dParamStopERP, erp1); - //dJointSetHingeParam(jointId, dParamCFM, cfm); - //dJointSetHingeParam(jointId, dParamFudgeFactor, 1); - } - else if(lo1 != 0) { - dJointSetHingeParam(jointId, dParamLoStop, lo1); - dJointSetHingeParam(jointId, dParamHiStop, hi1); - } - if(jointCFM > 0) { - dJointSetHingeParam(jointId, dParamCFM, jointCFM); - } - // good value for the SpaceClimber robot - //dJointSetHingeParam(jointId, dParamCFM, 0.03); - //dJointSetHingeParam(jointId, dParamCFM, 0.018); - //dJointSetHingeParam(jointId, dParamCFM, 0.09); - } - - /** - * \brief Creates a hinge2 joint in the physical environment - * - * pre: - * - body1 and body2 should be aviable - * - * post: - * - a hinge2 joint should be created and connected to the bodies - */ - void JointPhysics::createHinge2(JointData *jointS, dBodyID body1, - dBodyID body2){ - - jointId= dJointCreateHinge2(theWorld->getWorld(),0); - dJointAttach(jointId, body1, body2); - dJointSetHinge2Anchor(jointId, jointS->anchor.x(), jointS->anchor.y(), - jointS->anchor.z()); - dJointSetHinge2Axis1(jointId, jointS->axis1.x(), jointS->axis1.y(), - jointS->axis1.z()); - dJointSetHinge2Axis2(jointId, jointS->axis2.x(), jointS->axis2.y(), - jointS->axis2.z()); - - if(damping>0.00000001) { - dJointSetHinge2Param(jointId, dParamFMax, damping); - dJointSetHinge2Param(jointId, dParamVel, 0); - dJointSetHinge2Param(jointId, dParamFMax2, damping); - dJointSetHinge2Param(jointId, dParamVel2, 0); - } - if(spring > 0.00000001) { - dJointSetHinge2Param(jointId, dParamLoStop, lo1); - dJointSetHinge2Param(jointId, dParamHiStop, hi1); - dJointSetHinge2Param(jointId, dParamLoStop2, lo2); - dJointSetHinge2Param(jointId, dParamHiStop2, hi2); - dJointSetHinge2Param(jointId, dParamStopCFM, cfm1); - dJointSetHinge2Param(jointId, dParamStopERP, erp1); - dJointSetHinge2Param(jointId, dParamStopCFM2, cfm2); - dJointSetHinge2Param(jointId, dParamStopERP2, erp2); - dJointSetHinge2Param(jointId, dParamCFM, cfm); - } - if(jointCFM > 0) { - dJointSetHinge2Param(jointId, dParamCFM, jointCFM); - } - } - - void JointPhysics::createSlider(JointData *jointS, dBodyID body1, - dBodyID body2){ - jointId= dJointCreateSlider(theWorld->getWorld(),0); - dJointAttach(jointId, body1, body2); - dJointSetSliderAxis(jointId, jointS->axis1.x(), jointS->axis1.y(), - jointS->axis1.z()); - - if(spring > 0.00000001) { - dJointSetSliderParam(jointId, dParamLoStop, lo1); - dJointSetSliderParam(jointId, dParamHiStop, hi1); - dJointSetSliderParam(jointId, dParamStopCFM, cfm1); - dJointSetSliderParam(jointId, dParamStopERP, erp1); - } - else if(damping > 0.00000001) { - dJointSetSliderParam(jointId, dParamFMax, damping); - dJointSetSliderParam(jointId, dParamVel, 0); - } - else if(lo1 != 0) { - dJointSetSliderParam(jointId, dParamLoStop, lo1); - dJointSetSliderParam(jointId, dParamHiStop, hi1); - } - if(jointCFM > 0) { - dJointSetSliderParam(jointId, dParamCFM, jointCFM); - } - //dJointSetSliderParam(jointId, dParamCFM, cfm); - } - - void JointPhysics::createBall(JointData *jointS, dBodyID body1, - dBodyID body2){ - jointId= dJointCreateBall(theWorld->getWorld(),0); - dJointAttach(jointId, body1, body2); - dJointSetBallAnchor(jointId, jointS->anchor.x(), jointS->anchor.y(), - jointS->anchor.z()); - dJointSetBallParam(jointId, dParamCFM, cfm1); - dJointSetBallParam(jointId, dParamERP, erp1); - /* - ball_motor = dJointCreateAMotor(theWorld->getWorld(), 0); - dJointAttach(ball_motor, body1, body2); - dJointSetAMotorNumAxes(ball_motor, 3); - dJointSetAMotorAxis(ball_motor, 0, 1, 1,0,0); - dJointSetAMotorAxis(ball_motor, 2, 2, 0,1,0); - - if(damping > 0.00000001) { - dJointSetAMotorParam(ball_motor, dParamVel, 0); - dJointSetAMotorParam(ball_motor, dParamVel2, 0); - dJointSetAMotorParam(ball_motor, dParamVel3, 0); - dJointSetAMotorParam(ball_motor, dParamFMax, damping); - dJointSetAMotorParam(ball_motor, dParamFMax2, damping); - dJointSetAMotorParam(ball_motor, dParamFMax3, damping); - } - else if(spring > 0.00000001) { - dJointSetAMotorParam(ball_motor, dParamLoStop, lo2); - dJointSetAMotorParam(ball_motor, dParamLoStop2, lo2); - dJointSetAMotorParam(ball_motor, dParamLoStop3, lo2); - dJointSetAMotorParam(ball_motor, dParamHiStop, hi1); - dJointSetAMotorParam(ball_motor, dParamHiStop2, hi2); - dJointSetAMotorParam(ball_motor, dParamHiStop3, hi2); - dJointSetAMotorParam(ball_motor, dParamCFM, cfm1); - dJointSetAMotorParam(ball_motor, dParamCFM2, cfm2); - dJointSetAMotorParam(ball_motor, dParamCFM3, cfm2); - dJointSetAMotorParam(ball_motor, dParamERP, erp1); - dJointSetAMotorParam(ball_motor, dParamERP2, erp2); - dJointSetAMotorParam(ball_motor, dParamERP3, erp2); - } - dJointSetAMotorMode (ball_motor, dAMotorEuler); - */ - } - - /** - * \brief Creates a universal joint in the physical environment - * - * pre: - * - body1 and body2 should be aviable - * - * post: - * - a universal joint should be created and connected to the bodies - */ - void JointPhysics::createUniversal(JointData *jointS, dBodyID body1, - dBodyID body2){ - - jointId= dJointCreateUniversal(theWorld->getWorld(),0); - dJointAttach(jointId, body1, body2); - dJointSetUniversalAnchor(jointId, jointS->anchor.x(), jointS->anchor.y(), - jointS->anchor.z()); - dJointSetUniversalAxis1(jointId, jointS->axis1.x(), jointS->axis1.y(), - jointS->axis1.z()); - dJointSetUniversalAxis2(jointId, jointS->axis2.x(), jointS->axis2.y(), - jointS->axis2.z()); - - if(damping>0.00000001) { - dJointSetUniversalParam(jointId, dParamFMax, damping); - dJointSetUniversalParam(jointId, dParamVel, 0); - dJointSetUniversalParam(jointId, dParamFMax2, damping); - dJointSetUniversalParam(jointId, dParamVel2, 0); - } - if(spring > 0.00000001) { - dJointSetUniversalParam(jointId, dParamLoStop, lo1); - dJointSetUniversalParam(jointId, dParamHiStop, hi1); - dJointSetUniversalParam(jointId, dParamLoStop2, lo2); - dJointSetUniversalParam(jointId, dParamHiStop2, hi2); - dJointSetUniversalParam(jointId, dParamStopCFM, cfm1); - dJointSetUniversalParam(jointId, dParamStopERP, erp1); - dJointSetUniversalParam(jointId, dParamStopCFM2, cfm2); - dJointSetUniversalParam(jointId, dParamStopERP2, erp2); - dJointSetUniversalParam(jointId, dParamCFM, cfm); - } - if(lo1 > 0.0001 || lo1 < -0.0001) - dJointSetUniversalParam(jointId, dParamLoStop, lo1); - if(lo2 > 0.0001 || lo2 < -0.0001) - dJointSetUniversalParam(jointId, dParamHiStop, hi1); - if(hi1 > 0.0001 || hi1 < -0.0001) - dJointSetUniversalParam(jointId, dParamLoStop2, lo2); - if(hi2 > 0.0001 || hi2 < -0.0001) - dJointSetUniversalParam(jointId, dParamHiStop2, hi2); - if(jointCFM > 0) { - dJointSetUniversalParam(jointId, dParamCFM, jointCFM); - } - } - /// set the anchor i.e. the position where the joint is created of the joint - void JointPhysics::setAnchor(const Vector &anchor){ + void ODEJoint::setAnchor(const Vector &anchor){ MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { @@ -550,7 +330,7 @@ namespace mars { * * post: */ - void JointPhysics::setAxis(const Vector &axis){ + void ODEJoint::setAxis(const Vector &axis){ MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { @@ -582,7 +362,7 @@ namespace mars { * * post: */ - void JointPhysics::setAxis2(const Vector &axis){ + void ODEJoint::setAxis2(const Vector &axis){ MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { case JOINT_TYPE_HINGE: @@ -615,7 +395,7 @@ namespace mars { * post: * - the given axis struct should be filled with correct values */ - void JointPhysics::getAxis(Vector* axis) const { + void ODEJoint::getAxis(Vector* axis) const { dReal pos[4] = {0,0,0,0}; MutexLocker locker(&(theWorld->iMutex)); @@ -653,7 +433,7 @@ namespace mars { * post: * - the given axis struct should be filled with correct values */ - void JointPhysics::getAxis2(Vector* axis) const { + void ODEJoint::getAxis2(Vector* axis) const { dReal pos[4] = {0,0,0,0}; MutexLocker locker(&(theWorld->iMutex)); @@ -683,11 +463,11 @@ namespace mars { } ///set the world informations - void JointPhysics::setWorldObject(std::shared_ptr world){ + void ODEJoint::setWorldObject(std::shared_ptr world){ theWorld = std::static_pointer_cast(world); } - void JointPhysics::setJointAsMotor(int axis) { + void ODEJoint::setJointAsMotor(int axis) { MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { // todo: need to handle the distinction whether to set or not to @@ -730,7 +510,7 @@ namespace mars { } } - void JointPhysics::unsetJointAsMotor(int axis) { + void ODEJoint::unsetJointAsMotor(int axis) { MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { case JOINT_TYPE_HINGE: @@ -773,7 +553,7 @@ namespace mars { * post: * */ - void JointPhysics::getForce1(Vector *f) const { + void ODEJoint::getForce1(Vector *f) const { f->x() = (sReal)feedback.f1[0]; f->y() = (sReal)feedback.f1[1]; f->z() = (sReal)feedback.f1[2]; @@ -788,7 +568,7 @@ namespace mars { * post: * */ - void JointPhysics::getForce2(Vector *f) const { + void ODEJoint::getForce2(Vector *f) const { f->x() = (sReal)feedback.f2[0]; f->y() = (sReal)feedback.f2[1]; f->z() = (sReal)feedback.f2[2]; @@ -803,7 +583,7 @@ namespace mars { * post: * */ - void JointPhysics::getTorque1(Vector *t) const { + void ODEJoint::getTorque1(Vector *t) const { t->x() = (sReal)feedback.t1[0]; t->y() = (sReal)feedback.t1[1]; t->z() = (sReal)feedback.t1[2]; @@ -818,7 +598,7 @@ namespace mars { * post: * */ - void JointPhysics::getTorque2(Vector *t) const { + void ODEJoint::getTorque2(Vector *t) const { t->x() = (sReal)feedback.t2[0]; t->y() = (sReal)feedback.t2[1]; t->z() = (sReal)feedback.t2[2]; @@ -835,7 +615,7 @@ namespace mars { * post: * */ - void JointPhysics::reattacheJoint(void) { + void ODEJoint::reattacheJoint(void) { dReal pos[4] = {0,0,0,0}; MutexLocker locker(&(theWorld->iMutex)); @@ -893,13 +673,13 @@ namespace mars { * normal[1] *= dot*radius; * normal[2] *= dot*radius; */ - void JointPhysics::getAxisTorque(Vector *t) const { + void ODEJoint::getAxisTorque(Vector *t) const { t->x() = axis1_torque.x(); t->y() = axis1_torque.y(); t->z() = axis1_torque.z(); } - void JointPhysics::getAxis2Torque(Vector *t) const { + void ODEJoint::getAxis2Torque(Vector *t) const { t->x() = axis2_torque.x(); t->y() = axis2_torque.y(); t->z() = axis2_torque.z(); @@ -910,7 +690,7 @@ namespace mars { * we can return it. * */ - void JointPhysics::update(void) { + void ODEJoint::update(void) { const dReal *b1_pos, *b2_pos; dReal anchor[4], axis[4], axis2[4]; int calc1 = 0, calc2 = 0; @@ -1064,38 +844,13 @@ namespace mars { } } - void JointPhysics::getJointLoad(Vector *t) const { + void ODEJoint::getJointLoad(Vector *t) const { t->x() = joint_load.x(); t->y() = joint_load.y(); t->z() = joint_load.z(); } - - /** - * \brief Creates a fixed joint in the physical environment. For - * static fixed objects a same group id is prefered, the fixed - * joint should only be used to dynamically connect and unconnect nodes. - * - * pre: - * - body1 and/or body2 should be aviable - * - * post: - * - a fixed joint should be created and connected to the bodies - */ - void JointPhysics::createFixed(JointData *jointS, dBodyID body1, - dBodyID body2){ - CPP_UNUSED(jointS); - jointId = dJointCreateFixed(theWorld->getWorld(), 0); - dJointAttach(jointId, body1, body2); - dJointSetFixed(jointId); - // used for the integration study of the SpaceClimber - //dJointSetFixedParam(jointId, dParamCFM, cfm1);//0.0002); - //dJointSetFixedParam(jointId, dParamERP, erp1);//0.0002); - //dJointSetFixedParam(jointId, dParamCFM, 0.001); - } - - - sReal JointPhysics::getVelocity(void) const { + sReal ODEJoint::getVelocity(void) const { MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { case JOINT_TYPE_HINGE: @@ -1120,7 +875,7 @@ namespace mars { return 0; } - sReal JointPhysics::getVelocity2(void) const { + sReal ODEJoint::getVelocity2(void) const { MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { case JOINT_TYPE_HINGE: @@ -1143,7 +898,7 @@ namespace mars { return 0; } - void JointPhysics::setTorque(sReal torque) { + void ODEJoint::setTorque(sReal torque) { MutexLocker locker(&(theWorld->iMutex)); switch(joint_type) { case JOINT_TYPE_HINGE: @@ -1166,7 +921,7 @@ namespace mars { } } - void JointPhysics::setTorque2(sReal torque) { + void ODEJoint::setTorque2(sReal torque) { CPP_UNUSED(torque); switch(joint_type) { case JOINT_TYPE_HINGE: @@ -1186,7 +941,7 @@ namespace mars { } } - void JointPhysics::changeStepSize(const JointData &jointS) { + void ODEJoint::changeStepSize(const JointData &jointS) { MutexLocker locker(&(theWorld->iMutex)); if(theWorld && theWorld->existsWorld()) { calculateCfmErp(&jointS); @@ -1295,11 +1050,11 @@ namespace mars { } } - sReal JointPhysics::getMotorTorque(void) const { + sReal ODEJoint::getMotorTorque(void) const { return motor_torque; } - interfaces::sReal JointPhysics::getLowStop() const { + interfaces::sReal ODEJoint::getLowStop() const { switch(joint_type) { case JOINT_TYPE_HINGE: return dJointGetHingeParam(jointId, dParamLoStop); @@ -1309,12 +1064,12 @@ namespace mars { return dJointGetSliderParam(jointId, dParamLoStop); default: // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: lowstop for type %d not implemented yet\n", joint_type); + fprintf(stderr, "mars::sim::ODEJoint: lowstop for type %d not implemented yet\n", joint_type); return 0.; } } - interfaces::sReal JointPhysics::getHighStop() const { + interfaces::sReal ODEJoint::getHighStop() const { switch(joint_type) { case JOINT_TYPE_HINGE: return dJointGetHingeParam(jointId, dParamHiStop); @@ -1324,34 +1079,34 @@ namespace mars { return dJointGetSliderParam(jointId, dParamHiStop); default: // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: highstop for type %d not implemented yet\n", joint_type); + fprintf(stderr, "mars::sim::ODEJoint: highstop for type %d not implemented yet\n", joint_type); return 0.; } } - interfaces::sReal JointPhysics::getLowStop2() const { + interfaces::sReal ODEJoint::getLowStop2() const { switch(joint_type) { case JOINT_TYPE_HINGE2: return dJointGetHinge2Param(jointId, dParamLoStop2); default: // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: lowstop for type %d not implemented yet\n", joint_type); + fprintf(stderr, "mars::sim::ODEJoint: lowstop for type %d not implemented yet\n", joint_type); return 0.; } } - interfaces::sReal JointPhysics::getHighStop2() const { + interfaces::sReal ODEJoint::getHighStop2() const { switch(joint_type) { case JOINT_TYPE_HINGE2: return dJointGetHinge2Param(jointId, dParamHiStop2); default: // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: highstop for type %d not implemented yet\n", joint_type); + fprintf(stderr, "mars::sim::ODEJoint: highstop for type %d not implemented yet\n", joint_type); return 0.; } } - interfaces::sReal JointPhysics::getCFM() const { + interfaces::sReal ODEJoint::getCFM() const { switch(joint_type) { case JOINT_TYPE_HINGE: return dJointGetHingeParam(jointId, dParamCFM); @@ -1361,12 +1116,12 @@ namespace mars { return dJointGetSliderParam(jointId, dParamCFM); default: // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: cfm for type %d not implemented yet\n", joint_type); + fprintf(stderr, "mars::sim::ODEJoint: cfm for type %d not implemented yet\n", joint_type); return 0.; } } - void JointPhysics::setLowStop(interfaces::sReal lowStop) { + void ODEJoint::setLowStop(interfaces::sReal lowStop) { switch(joint_type) { case JOINT_TYPE_HINGE: dJointSetHingeParam(jointId, dParamLoStop, lowStop); @@ -1379,12 +1134,12 @@ namespace mars { break; default: // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: highstop for type %d not implemented yet\n", joint_type); + fprintf(stderr, "mars::sim::ODEJoint: highstop for type %d not implemented yet\n", joint_type); break; } } - void JointPhysics::setHighStop(interfaces::sReal highStop) { + void ODEJoint::setHighStop(interfaces::sReal highStop) { switch(joint_type) { case JOINT_TYPE_HINGE: dJointSetHingeParam(jointId, dParamHiStop, highStop); @@ -1397,36 +1152,36 @@ namespace mars { break; default: // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: highstop for type %d not implemented yet\n", joint_type); + fprintf(stderr, "mars::sim::ODEJoint: highstop for type %d not implemented yet\n", joint_type); break; } } - void JointPhysics::setLowStop2(interfaces::sReal lowStop) { + void ODEJoint::setLowStop2(interfaces::sReal lowStop) { switch(joint_type) { case JOINT_TYPE_HINGE2: dJointSetHinge2Param(jointId, dParamLoStop, lowStop); break; default: // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: highstop for type %d not implemented yet\n", joint_type); + fprintf(stderr, "mars::sim::ODEJoint: highstop for type %d not implemented yet\n", joint_type); break; } } - void JointPhysics::setHighStop2(interfaces::sReal highStop) { + void ODEJoint::setHighStop2(interfaces::sReal highStop) { switch(joint_type) { case JOINT_TYPE_HINGE2: dJointSetHinge2Param(jointId, dParamHiStop2, highStop); break; default: // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: highstop for type %d not implemented yet\n", joint_type); + fprintf(stderr, "mars::sim::ODEJoint: highstop for type %d not implemented yet\n", joint_type); break; } } - void JointPhysics::setCFM(interfaces::sReal cfm) { + void ODEJoint::setCFM(interfaces::sReal cfm) { switch(joint_type) { case JOINT_TYPE_HINGE: dJointSetHingeParam(jointId, dParamCFM, cfm); @@ -1439,10 +1194,10 @@ namespace mars { break; default: // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: cfm for type %d not implemented yet\n", joint_type); + fprintf(stderr, "mars::sim::ODEJoint: cfm for type %d not implemented yet\n", joint_type); break; } } - } // end of namespace sim +} // end of namespace sim } // end of namespace mars diff --git a/sim/src/physics/JointPhysics.h b/sim/src/physics/joints/ODEJoint.h similarity index 80% rename from sim/src/physics/JointPhysics.h rename to sim/src/physics/joints/ODEJoint.h index fb1dfd63b..bf49ced82 100644 --- a/sim/src/physics/JointPhysics.h +++ b/sim/src/physics/joints/ODEJoint.h @@ -18,11 +18,11 @@ * */ -#ifndef JOINT_PHYSICS_H -#define JOINT_PHYSICS_H +#ifndef ODE_JOINT_H +#define ODE_JOINT_H #ifdef _PRINT_HEADER_ - #warning "JointPhysics.h" + #warning "ODEJoint.h" #endif #include "WorldPhysics.h" @@ -31,19 +31,21 @@ #include namespace mars { - namespace sim { +namespace sim { - class JointPhysics: public interfaces::JointInterface { + class ODEJoint: public interfaces::JointInterface { public: ///the constructor - JointPhysics(std::shared_ptr world); + ODEJoint(std::shared_ptr world); ///the destructor - virtual ~JointPhysics(void); + virtual ~ODEJoint(void); ///create Joint getting as argument the JointData which give the ///joint informations. virtual bool createJoint(interfaces::JointData *joint, const std::shared_ptr node1, const std::shared_ptr node2); + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2); + ///get the anchor of the joint virtual void getAnchor(utils::Vector* anchor) const; /// set the anchor i.e. the position where the joint is created of the joint @@ -88,8 +90,9 @@ namespace mars { virtual void setLowStop2(interfaces::sReal lowStop2); virtual void setHighStop2(interfaces::sReal highStop2); virtual void setCFM(interfaces::sReal cfm); + bool isJointCreated(); - private: + protected: std::shared_ptr theWorld; dJointID jointId, ball_motor; dJointFeedback feedback; @@ -100,25 +103,13 @@ namespace mars { dReal damping, spring, jointCFM; utils::Vector axis1_torque, axis2_torque, joint_load; dReal motor_torque; - - void calculateCfmErp(const interfaces::JointData *jointS); - - ///create a joint from type Hing - void createHinge(interfaces::JointData* jointS, - dBodyID body1, dBodyID body2); - void createHinge2(interfaces::JointData* jointS, - dBodyID body1, dBodyID body2); - void createSlider(interfaces::JointData* jointS, - dBodyID body1, dBodyID body2); - void createBall(interfaces::JointData* jointS, - dBodyID body1, dBodyID body2); - void createUniversal(interfaces::JointData* jointS, - dBodyID body1, dBodyID body2); - void createFixed(interfaces::JointData* jointS, - dBodyID body1, dBodyID body2); + void calculateCfmErp(const interfaces::JointData *jointS); + + private: + bool joint_created; }; - } // end of namespace sim +} // end of namespace sim } // end of namespace mars -#endif +#endif // ODE_JOINT_H diff --git a/sim/src/physics/joints/ODEJointFactory.cpp b/sim/src/physics/joints/ODEJointFactory.cpp new file mode 100644 index 000000000..1e4171502 --- /dev/null +++ b/sim/src/physics/joints/ODEJointFactory.cpp @@ -0,0 +1,74 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "joints/ODEJointFactory.h" +#include "joints/ODEHingeJoint.h" +#include "joints/ODEHinge2Joint.h" +#include "joints/ODESliderJoint.h" +#include "joints/ODEBallJoint.h" +#include "joints/ODEUniversalJoint.h" +#include "joints/ODEFixedJoint.h" +#include + +#include + +namespace mars{ +namespace sim{ + +using namespace ::mars::interfaces; + +ODEJointFactory& ODEJointFactory::Instance(){ + static ODEJointFactory instance; + return instance; +} + +ODEJointFactory::ODEJointFactory(){ +} + +ODEJointFactory::~ODEJointFactory(){ +} + +std::shared_ptr ODEJointFactory::createJoint(std::shared_ptr worldPhysics, + interfaces::JointData *jointData, + const std::shared_ptr node1, + const std::shared_ptr node2){ + std::map::iterator it = availableJoints.find(std::string(getJointTypeString(jointData->type))); + if(it == availableJoints.end()){ + throw std::runtime_error("Could not load unknown Physics Joint with name: \"" + jointData->name + "\"" ); + } + + std::shared_ptr newJoint = std::make_shared(*(it->second(worldPhysics, jointData, node1, node2))); + if(newJoint->isJointCreated()){ + return newJoint; + } + else{ + std::cerr << "Failed to create Physics Joint with name: \"" + jointData->name + "\"" << std::endl; + return std::shared_ptr(nullptr); + } + +} + +void ODEJointFactory::addJointType(const std::string& type, instantiateJointfPtr funcPtr){ + + availableJoints.insert(std::pair(type, funcPtr)); +} + +} +} diff --git a/sim/src/physics/joints/ODEJointFactory.h b/sim/src/physics/joints/ODEJointFactory.h new file mode 100644 index 000000000..f62ced994 --- /dev/null +++ b/sim/src/physics/joints/ODEJointFactory.h @@ -0,0 +1,60 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODEJointFactory.h + * \author Muhammad Haider Khan Lodhi + * \brief "ODEJointFactory" implements JointFactoryInterface interface to create ode joints for the nodes. + * + */ + +#pragma once + +#include "joints/JointFactoryInterface.h" +#include "joints/ODEJoint.h" + +namespace mars{ +namespace sim{ + +using namespace ::mars::interfaces; + +typedef ODEJoint* (*instantiateJointfPtr)(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + +class ODEJointFactory : public JointFactoryInterface{ +public: + static ODEJointFactory& Instance(); + virtual std::shared_ptr createJoint(std::shared_ptr worldPhysics, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2) override; + void addJointType(const std::string& type, instantiateJointfPtr funcPtr); + +protected: + ODEJointFactory(); + virtual ~ODEJointFactory(); + std::map availableJoints; + +}; + +} +} diff --git a/sim/src/physics/joints/ODESliderJoint.cpp b/sim/src/physics/joints/ODESliderJoint.cpp new file mode 100644 index 000000000..785ae1907 --- /dev/null +++ b/sim/src/physics/joints/ODESliderJoint.cpp @@ -0,0 +1,76 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODESliderJoint.h" + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODESliderJoint::ODESliderJoint(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2) + : ODEJoint(world) { + createJoint(joint, node1, node2 ); + } + + ODESliderJoint::~ODESliderJoint(void) { + } + + ODEJoint* ODESliderJoint::instanciate(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2){ + return new ODESliderJoint(world, joint, node1, node2); + } + + bool ODESliderJoint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2){ + + jointId= dJointCreateSlider(theWorld->getWorld(),0); + dJointAttach(jointId, body1, body2); + dJointSetSliderAxis(jointId, jointS->axis1.x(), jointS->axis1.y(), + jointS->axis1.z()); + + if(spring > 0.00000001) { + dJointSetSliderParam(jointId, dParamLoStop, lo1); + dJointSetSliderParam(jointId, dParamHiStop, hi1); + dJointSetSliderParam(jointId, dParamStopCFM, cfm1); + dJointSetSliderParam(jointId, dParamStopERP, erp1); + } + else if(damping > 0.00000001) { + dJointSetSliderParam(jointId, dParamFMax, damping); + dJointSetSliderParam(jointId, dParamVel, 0); + } + else if(lo1 != 0) { + dJointSetSliderParam(jointId, dParamLoStop, lo1); + dJointSetSliderParam(jointId, dParamHiStop, hi1); + } + if(jointCFM > 0) { + dJointSetSliderParam(jointId, dParamCFM, jointCFM); + } + //dJointSetSliderParam(jointId, dParamCFM, cfm); + return 1; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/joints/ODESliderJoint.h b/sim/src/physics/joints/ODESliderJoint.h new file mode 100644 index 000000000..27ce1950c --- /dev/null +++ b/sim/src/physics/joints/ODESliderJoint.h @@ -0,0 +1,63 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODESliderJoint.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_SLIDER_JOINT_H +#define ODE_SLIDER_JOINT_H + +#ifdef _PRINT_HEADER_ + #warning "ODESliderJoint.h" +#endif + +#include +#include "ODEJoint.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODESliderJoint : public ODEJoint { + public: + ODESliderJoint(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + virtual ~ODESliderJoint(void); + static ODEJoint* instanciate(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_SLIDER_JOINT_H diff --git a/sim/src/physics/joints/ODEUniversalJoint.cpp b/sim/src/physics/joints/ODEUniversalJoint.cpp new file mode 100644 index 000000000..559495887 --- /dev/null +++ b/sim/src/physics/joints/ODEUniversalJoint.cpp @@ -0,0 +1,94 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODEUniversalJoint.h" + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEUniversalJoint::ODEUniversalJoint(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2) + : ODEJoint(world) { + createJoint(joint, node1, node2 ); + } + + ODEUniversalJoint::~ODEUniversalJoint(void) { + } + + ODEJoint* ODEUniversalJoint::instanciate(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2){ + return new ODEUniversalJoint(world, joint, node1, node2); + } + + bool ODEUniversalJoint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) { + if (body1 || body2){ + jointId= dJointCreateUniversal(theWorld->getWorld(),0); + dJointAttach(jointId, body1, body2); + dJointSetUniversalAnchor(jointId, jointS->anchor.x(), jointS->anchor.y(), + jointS->anchor.z()); + dJointSetUniversalAxis1(jointId, jointS->axis1.x(), jointS->axis1.y(), + jointS->axis1.z()); + dJointSetUniversalAxis2(jointId, jointS->axis2.x(), jointS->axis2.y(), + jointS->axis2.z()); + + if(damping>0.00000001) { + dJointSetUniversalParam(jointId, dParamFMax, damping); + dJointSetUniversalParam(jointId, dParamVel, 0); + dJointSetUniversalParam(jointId, dParamFMax2, damping); + dJointSetUniversalParam(jointId, dParamVel2, 0); + } + if(spring > 0.00000001) { + dJointSetUniversalParam(jointId, dParamLoStop, lo1); + dJointSetUniversalParam(jointId, dParamHiStop, hi1); + dJointSetUniversalParam(jointId, dParamLoStop2, lo2); + dJointSetUniversalParam(jointId, dParamHiStop2, hi2); + dJointSetUniversalParam(jointId, dParamStopCFM, cfm1); + dJointSetUniversalParam(jointId, dParamStopERP, erp1); + dJointSetUniversalParam(jointId, dParamStopCFM2, cfm2); + dJointSetUniversalParam(jointId, dParamStopERP2, erp2); + dJointSetUniversalParam(jointId, dParamCFM, cfm); + } + if(lo1 > 0.0001 || lo1 < -0.0001) + dJointSetUniversalParam(jointId, dParamLoStop, lo1); + if(lo2 > 0.0001 || lo2 < -0.0001) + dJointSetUniversalParam(jointId, dParamHiStop, hi1); + if(hi1 > 0.0001 || hi1 < -0.0001) + dJointSetUniversalParam(jointId, dParamLoStop2, lo2); + if(hi2 > 0.0001 || hi2 < -0.0001) + dJointSetUniversalParam(jointId, dParamHiStop2, hi2); + if(jointCFM > 0) { + dJointSetUniversalParam(jointId, dParamCFM, jointCFM); + } + } + else { + return 0; + } + return 1; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/joints/ODEUniversalJoint.h b/sim/src/physics/joints/ODEUniversalJoint.h new file mode 100644 index 000000000..44a3f8b9b --- /dev/null +++ b/sim/src/physics/joints/ODEUniversalJoint.h @@ -0,0 +1,63 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODEUniversalJoint.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_UNIVERSAL_JOINT_H +#define ODE_UNIVERSAL_JOINT_H + +#ifdef _PRINT_HEADER_ + #warning "ODEUniversalJoint.h" +#endif + +#include +#include "ODEJoint.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODEUniversalJoint : public ODEJoint { + public: + ODEUniversalJoint(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + virtual ~ODEUniversalJoint(void); + static ODEJoint* instanciate(std::shared_ptr world, + interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2); + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_UNIVERSAL_JOINT_H diff --git a/sim/src/physics/notes.md b/sim/src/physics/notes.md new file mode 100644 index 000000000..eba8a1f0a --- /dev/null +++ b/sim/src/physics/notes.md @@ -0,0 +1,25 @@ +**TODOS:** + * make NodePhysics => ODEObject class interface + * create child classes for other node types similar to ODEBox + * check if destructor if ODEObject is called correctly + => move around object in scene. This might call setPosition, which destroys and createsObject + + * at the moment, we would still have the switch case in the PhysicsMapper newODEObject method. + * This should later on be replaced by a call to some Registry to request if a requested type is available and get it + * Malte has an example for this in some Sensor Class somewhere + + * check if all original NodePhysics functions are integrated into the ODEObject/ODE\* classes. Especially the heightmap callback + * think about seperating nBody (dynamic ODE) and nGeometry (collision) at some point + +**Style:** + * use smart pointers instead of free etc.. + * check constant correctness + * sort functions in header files and add doxygen documentation + * introduce LOG_DEBUG instead of printfs + +**QUESTIONS:** + * Talk about coding style and naming + * why is SimNode not inheriting from NodeInterface or another similar base class that contains all the get/set Force/Torque/Velocity stuff? + +**NOTES:** + d => ODEMethods diff --git a/sim/src/physics/objects/ODEBox.cpp b/sim/src/physics/objects/ODEBox.cpp new file mode 100644 index 000000000..506e26148 --- /dev/null +++ b/sim/src/physics/objects/ODEBox.cpp @@ -0,0 +1,73 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODEBox.h" +#include + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEBox::ODEBox(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + createNode(nodeData); + } + + ODEBox::~ODEBox(void) { + } + + ODEObject* ODEBox::instanciate(std::shared_ptr world, interfaces::NodeData * nodeData){ + return new ODEBox(world, nodeData); + } + + bool ODEBox::createODEGeometry(interfaces::NodeData *node){ + if (!node->inertia_set && + (node->ext.x() <= 0 || node->ext.y() <= 0 || node->ext.z() <= 0)) { + LOG_ERROR("Cannot create Node \"%s\" (id=%lu):\n" + " Box Nodes must have ext.x(), ext.y(), and ext.z() > 0.\n" + " Current values are: x=%g; y=%g, z=%g", + node->name.c_str(), node->index, + node->ext.x(), node->ext.y(), node->ext.z()); + return false; + } + + // build the ode representation + nGeom = dCreateBox(theWorld->getSpace(), (dReal)(node->ext.x()), + (dReal)(node->ext.y()), (dReal)(node->ext.z())); + + // create the mass object for the box + if(node->inertia_set) { + setInertiaMass(node); + } + else if(node->density > 0) { + dMassSetBox(&nMass, (dReal)(node->density), (dReal)(node->ext.x()), + (dReal)(node->ext.y()),(dReal)(node->ext.z())); + } + else if(node->mass > 0) { + dReal tempMass =(dReal)(node->mass); + dMassSetBoxTotal(&nMass, tempMass, (dReal)(node->ext.x()), + (dReal)(node->ext.y()),(dReal)(node->ext.z())); + } + return true; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/objects/ODEBox.h b/sim/src/physics/objects/ODEBox.h new file mode 100644 index 000000000..ece9fa61e --- /dev/null +++ b/sim/src/physics/objects/ODEBox.h @@ -0,0 +1,59 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODEBox.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_BOX_H +#define ODE_BOX_H + +#ifdef _PRINT_HEADER_ + #warning "ODEBox.h" +#endif + +#include +#include "ODEObject.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODEBox : public ODEObject { + public: + ODEBox(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEBox(void); + static ODEObject* instanciate(std::shared_ptr world, interfaces::NodeData * nodeData); + //TODO createGeometry vs createCollision? nBody vs nCollision + // review header comment on ODEBox + virtual bool createODEGeometry(interfaces::NodeData *node) override; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_BOX_H diff --git a/sim/src/physics/objects/ODECapsule.cpp b/sim/src/physics/objects/ODECapsule.cpp new file mode 100644 index 000000000..8838cdf21 --- /dev/null +++ b/sim/src/physics/objects/ODECapsule.cpp @@ -0,0 +1,71 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODECapsule.h" +#include + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODECapsule::ODECapsule(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + createNode(nodeData); + } + + ODECapsule::~ODECapsule(void) { + } + + ODEObject* ODECapsule::instanciate(std::shared_ptr world, interfaces::NodeData * nodeData){ + return new ODECapsule(world, nodeData); + } + + bool ODECapsule::createODEGeometry(interfaces::NodeData *node){ + if (!node->inertia_set && (node->ext.x() <= 0 || node->ext.y() <= 0)) { + LOG_ERROR("Cannot create Node \"%s\" (id=%lu):\n" + " Capsule Nodes must have ext.x() and ext.y() > 0.\n" + " Current values are: x=%g; y=%g", + node->name.c_str(), node->index, + node->ext.x(), node->ext.y()); + return false; + } + + // build the ode representation + nGeom = dCreateCapsule(theWorld->getSpace(), (dReal)node->ext.x(), + (dReal)node->ext.y()); + + // create the mass object for the capsule + if(node->inertia_set) { + setInertiaMass(node); + } + else if(node->density > 0) { + dMassSetCapsule(&nMass, (dReal)node->density, 3, (dReal)node->ext.x(), + (dReal)node->ext.y()); + } + else if(node->mass > 0) { + dMassSetCapsuleTotal(&nMass, (dReal)node->mass, 3, (dReal)node->ext.x(), + (dReal)node->ext.y()); + } + return true; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/objects/ODECapsule.h b/sim/src/physics/objects/ODECapsule.h new file mode 100644 index 000000000..815c2dc17 --- /dev/null +++ b/sim/src/physics/objects/ODECapsule.h @@ -0,0 +1,57 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODECapsule.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_CAPSULE_H +#define ODE_CAPSULE_H + +#ifdef _PRINT_HEADER_ + #warning "ODECapsule.h" +#endif + +#include +#include "ODEObject.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODECapsule : public ODEObject { + public: + ODECapsule(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODECapsule(void); + static ODEObject* instanciate(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual bool createODEGeometry(interfaces::NodeData *node) override; + }; + + } // end of namespace sim +} // end of namespace mars + +#endif // ODE_CAPSULE_H diff --git a/sim/src/physics/objects/ODECylinder.cpp b/sim/src/physics/objects/ODECylinder.cpp new file mode 100644 index 000000000..88d5427f0 --- /dev/null +++ b/sim/src/physics/objects/ODECylinder.cpp @@ -0,0 +1,69 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODECylinder.h" +#include + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODECylinder::ODECylinder(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + createNode(nodeData); + } + + ODECylinder::~ODECylinder(void) { + } + ODEObject* ODECylinder::instanciate(std::shared_ptr world, interfaces::NodeData * nodeData){ + return new ODECylinder(world, nodeData); + } + bool ODECylinder::createODEGeometry(interfaces::NodeData *node){ + if (!node->inertia_set && (node->ext.x() <= 0 || node->ext.y() <= 0)) { + LOG_ERROR("Cannot create Node \"%s\" (id=%lu):\n" + " Cylinder Nodes must have ext.x() and ext.y() > 0.\n" + " Current values are: x=%g; y=%g", + node->name.c_str(), node->index, + node->ext.x(), node->ext.y()); + return false; + } + + // build the ode representation + nGeom = dCreateCylinder(theWorld->getSpace(), (dReal)node->ext.x(), + (dReal)node->ext.y()); + + // create the mass object for the cylinder + if(node->inertia_set) { + setInertiaMass(node); + } + else if(node->density > 0) { + dMassSetCylinder(&nMass, (dReal)node->density, 3, (dReal)node->ext.x(), + (dReal)node->ext.y()); + } + else if(node->mass > 0) { + dMassSetCylinderTotal(&nMass, (dReal)node->mass, 3, (dReal)node->ext.x(), + (dReal)node->ext.y()); + } + return true; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/objects/ODECylinder.h b/sim/src/physics/objects/ODECylinder.h new file mode 100644 index 000000000..c3eb7184d --- /dev/null +++ b/sim/src/physics/objects/ODECylinder.h @@ -0,0 +1,57 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODECylinder.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_CYLINDER_H +#define ODE_CYLINDER_H + +#ifdef _PRINT_HEADER_ + #warning "ODECylinder.h" +#endif + +#include +#include "ODEObject.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODECylinder : public ODEObject { + public: + ODECylinder(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODECylinder(void); + static ODEObject* instanciate(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual bool createODEGeometry(interfaces::NodeData *node) override; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_CYLINDER_H diff --git a/sim/src/physics/objects/ODEHeightField.cpp b/sim/src/physics/objects/ODEHeightField.cpp new file mode 100644 index 000000000..854fdb33d --- /dev/null +++ b/sim/src/physics/objects/ODEHeightField.cpp @@ -0,0 +1,93 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODEHeightField.h" +#include + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEHeightField::ODEHeightField(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + height_data = 0; + createNode(nodeData); + } + + ODEHeightField::~ODEHeightField(void) { + if(height_data) free(height_data); + if(terrain) free (terrain); + } + + ODEObject* ODEHeightField::instanciate(std::shared_ptr world, interfaces::NodeData * nodeData){ + return new ODEHeightField(world, nodeData); + } + + dReal heightfield_callback(void* pUserData, int x, int z ) { + return ((ODEHeightField*)pUserData)->heightCallback(x, z); + } + + void ODEHeightField::destroyNode(void) { + ODEObject::destroyNode(); + height_data = 0; + } + + dReal ODEHeightField::heightCallback(int x, int y) { + + return (dReal)height_data[(y*terrain->width)+x]*terrain->scale; + } + + bool ODEHeightField::createODEGeometry(interfaces::NodeData *node){ + dMatrix3 R; + unsigned long size; + int x, y; + terrain = node->terrain; + size = terrain->width*terrain->height; + if(!height_data) height_data = (dReal*)calloc(size, sizeof(dReal)); + for(x=0; xheight; x++) { + for(y=0; ywidth; y++) { + height_data[(terrain->height-(x+1))*terrain->width+y] = (dReal)terrain->pixelData[x*terrain->width+y]; + } + } + // build the ode representation + dHeightfieldDataID heightid = dGeomHeightfieldDataCreate(); + + // Create an finite heightfield. + dGeomHeightfieldDataBuildCallback(heightid, this, heightfield_callback, + terrain->targetWidth, + terrain->targetHeight, + terrain->width, terrain->height, + REAL(1.0), REAL( 0.0 ), + REAL(1.0), 0); + // Give some very bounds which, while conservative, + // makes AABB computation more accurate than +/-INF. + dGeomHeightfieldDataSetBounds(heightid, REAL(-terrain->scale*2.0), + REAL(terrain->scale*2.0)); + //dGeomHeightfieldDataSetBounds(heightid, -terrain->scale, terrain->scale); + nGeom = dCreateHeightfield(theWorld->getSpace(), heightid, 1); + dRSetIdentity(R); + dRFromAxisAndAngle(R, 1, 0, 0, M_PI/2); + dGeomSetRotation(nGeom, R); + return true; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/objects/ODEHeightField.h b/sim/src/physics/objects/ODEHeightField.h new file mode 100644 index 000000000..6c1c043d5 --- /dev/null +++ b/sim/src/physics/objects/ODEHeightField.h @@ -0,0 +1,63 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODEHeightField.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_HEIGHT_FIELD_H +#define ODE_HEIGHT_FIELD_H + +#ifdef _PRINT_HEADER_ + #warning "ODEHeightField.h" +#endif + +#include +#include "ODEObject.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODEHeightField : public ODEObject { + public: + ODEHeightField(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEHeightField(void); + static ODEObject* instanciate(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual bool createODEGeometry(interfaces::NodeData *node) override; + virtual void destroyNode(void) override; + dReal heightCallback(int x, int y); + + protected: + interfaces::terrainStruct *terrain; + dReal *height_data; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_HEIGHT_FIELD_H diff --git a/sim/src/physics/objects/ODEMesh.cpp b/sim/src/physics/objects/ODEMesh.cpp new file mode 100644 index 000000000..4da82f86a --- /dev/null +++ b/sim/src/physics/objects/ODEMesh.cpp @@ -0,0 +1,135 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODEMesh.h" +#include + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEMesh::ODEMesh(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + createNode(nodeData); + } + + ODEMesh::~ODEMesh(void) { + if(myVertices) free(myVertices); + if(myIndices) free(myIndices); + if(myTriMeshData) dGeomTriMeshDataDestroy(myTriMeshData); + } + + ODEObject* ODEMesh::instanciate(std::shared_ptr world, interfaces::NodeData * nodeData){ + return new ODEMesh(world, nodeData); + } + + void ODEMesh::destroyNode(void) { + ODEObject::destroyNode(); + if(myVertices) free(myVertices); + if(myIndices) free(myIndices); + if(myTriMeshData) dGeomTriMeshDataDestroy(myTriMeshData); + myVertices = 0; + myIndices = 0; + myTriMeshData = 0; + } + + /** + * The method creates an ode box representation of the given node. + * + */ + bool ODEMesh::createODEGeometry(NodeData* node) { + int i; + + if (!node->inertia_set && + (node->ext.x() <= 0 || node->ext.y() <= 0 || node->ext.z() <= 0)) { + LOG_ERROR("Cannot create Node \"%s\" (id=%lu):\n" + " Mesh Nodes must have ext.x(), ext.y(), and ext.z() > 0.\n" + " Current values are: x=%g; y=%g, z=%g", + node->name.c_str(), node->index, + node->ext.x(), node->ext.y(), node->ext.z()); + return false; + } + + myVertices = (dVector3*)calloc(node->mesh.vertexcount, sizeof(dVector3)); + myIndices = (dTriIndex*)calloc(node->mesh.indexcount, sizeof(dTriIndex)); + //LOG_DEBUG("%d %d", node->mesh.vertexcount, node->mesh.indexcount); + // first we have to copy the mesh data to prevent errors in case + // of double to float conversion + dReal minx, miny, minz, maxx, maxy, maxz; + for(i=0; imesh.vertexcount; i++) { + myVertices[i][0] = (dReal)node->mesh.vertices[i][0]; + myVertices[i][1] = (dReal)node->mesh.vertices[i][1]; + myVertices[i][2] = (dReal)node->mesh.vertices[i][2]; + if(i==0) { + minx = myVertices[i][0]; + maxx = myVertices[i][0]; + miny = myVertices[i][1]; + maxy = myVertices[i][1]; + minz = myVertices[i][2]; + maxz = myVertices[i][2]; + } + else { + if(minx > myVertices[i][0]) minx = myVertices[i][0]; + if(maxx < myVertices[i][0]) maxx = myVertices[i][0]; + if(miny > myVertices[i][1]) miny = myVertices[i][1]; + if(maxy < myVertices[i][1]) maxy = myVertices[i][1]; + if(minz > myVertices[i][2]) minz = myVertices[i][2]; + if(maxz < myVertices[i][2]) maxz = myVertices[i][2]; + } + } + // rescale + dReal sx = node->ext.x()/(maxx-minx); + dReal sy = node->ext.y()/(maxy-miny); + dReal sz = node->ext.z()/(maxz-minz); + for(i=0; imesh.vertexcount; i++) { + myVertices[i][0] *= sx; + myVertices[i][1] *= sy; + myVertices[i][2] *= sz; + } + for(i=0; imesh.indexcount; i++) { + myIndices[i] = (dTriIndex)node->mesh.indices[i]; + } + + // then we can build the ode representation + myTriMeshData = dGeomTriMeshDataCreate(); + dGeomTriMeshDataBuildSimple(myTriMeshData, (dReal*)myVertices, + node->mesh.vertexcount, + myIndices, node->mesh.indexcount); + nGeom = dCreateTriMesh(theWorld->getSpace(), myTriMeshData, 0, 0, 0); + + // at this moment we set the mass properties as the mass of the + // bounding box if no mass and inertia is set by the user + if(node->inertia_set) { + setInertiaMass(node); + } + else if(node->density > 0) { + dMassSetBox(&nMass, (dReal)node->density, (dReal)node->ext.x(), + (dReal)node->ext.y(),(dReal)node->ext.z()); + } + else if(node->mass > 0) { + dMassSetBoxTotal(&nMass, (dReal)node->mass, (dReal)node->ext.x(), + (dReal)node->ext.y(),(dReal)node->ext.z()); + } + return true; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/objects/ODEMesh.h b/sim/src/physics/objects/ODEMesh.h new file mode 100644 index 000000000..3a62a9097 --- /dev/null +++ b/sim/src/physics/objects/ODEMesh.h @@ -0,0 +1,62 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODEMesh.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_MESH_H +#define ODE_MESH_H + +#ifdef _PRINT_HEADER_ + #warning "ODEMesh.h" +#endif + +#include +#include "ODEObject.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODEMesh : public ODEObject { + public: + ODEMesh(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEMesh(void); + static ODEObject* instanciate(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual bool createODEGeometry(interfaces::NodeData *node) override; + virtual void destroyNode(void) override; + protected: + dVector3 *myVertices; + dTriIndex *myIndices; + dTriMeshDataID myTriMeshData; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_MESH_H diff --git a/sim/src/physics/NodePhysics.cpp b/sim/src/physics/objects/ODEObject.cpp similarity index 74% rename from sim/src/physics/NodePhysics.cpp rename to sim/src/physics/objects/ODEObject.cpp index 2850497a5..583b3ed9b 100644 --- a/sim/src/physics/NodePhysics.cpp +++ b/sim/src/physics/objects/ODEObject.cpp @@ -1,5 +1,5 @@ /* - * Copyright 2011, 2012, DFKI GmbH Robotics Innovation Center + * Copyright 2022, DFKI GmbH Robotics Innovation Center * * This file is part of the MARS simulation framework. * @@ -18,31 +18,14 @@ * */ -/** - * \file NodePhysics.h - * \author Malte Roemmermann - * \brief "NodePhysics" implements the physical ode stuff for the nodes. - * - * - * ToDo: - * - Heightfield representation (valentin) - * - support for different spaces (later) - * - add impulse forces and velocities of nodes (later) - * - set an offset to the position (???) - * - set an offset to the rotation (???) - * - maybe some methods to support different sensors (???) - * - rotateAtPoint for composite and not move group *** - * - */ - -#include "NodePhysics.h" +//TODO cleanup includes +#include "ODEObject.h" #include "../sensors/RotatingRaySensor.h" #include #include #include #include -#include #include #include @@ -50,9 +33,11 @@ #include +#include + namespace mars { - namespace sim { +namespace sim { using namespace utils; using namespace interfaces; @@ -71,19 +56,15 @@ namespace mars { * - the class should have saved the pointer to the physics implementation * - the body and geom should be initialized to 0 */ - NodePhysics::NodePhysics(std::shared_ptr world) { - // At this moment we have not much things to do here. ^_^ + ODEObject::ODEObject(std::shared_ptr world, NodeData * nodeData) { theWorld = std::dynamic_pointer_cast(world); nBody = 0; nGeom = 0; - myVertices = 0; - myIndices = 0; - myTriMeshData = 0; composite = false; //node_data.num_ground_collisions = 0; - node_data.setZero(); - height_data = 0; + node_data.setZero(); dMassSetZero(&nMass); + object_created = false; } /** @@ -97,7 +78,7 @@ namespace mars { * * are the geom and the body realy all thing to take care of? */ - NodePhysics::~NodePhysics(void) { + ODEObject::~ODEObject(void) { std::vector::iterator iter; MutexLocker locker(&(theWorld->iMutex)); @@ -105,10 +86,6 @@ namespace mars { if(nGeom) dGeomDestroy(nGeom); - if(myVertices) free(myVertices); - if(myIndices) free(myIndices); - if(height_data) free(height_data); - // TODO: how does this loop work? why doesn't it run forever? for(iter = sensor_list.begin(); iter != sensor_list.end();) { if((*iter).gd){ @@ -118,27 +95,42 @@ namespace mars { dGeomDestroy((*iter).geom); sensor_list.erase(iter); } - if(myTriMeshData) dGeomTriMeshDataDestroy(myTriMeshData); } - dReal heightfield_callback(void* pUserData, int x, int z ) { - return ((NodePhysics*)pUserData)->heightCallback(x, z); + bool ODEObject::isObjectCreated(){ + return object_created; } /** - * \brief The method creates an ode node, which properties are given by - * the NodeData param node. + * \brief The method copies the position of the node at the adress + * of the pointer pos. * * pre: - * - Node sturct should point to a correct object - * - we should have a pointer to the physics implementation - * - a physically world should have been created to insert a node + * - the physical representation of the node should be availbe + * - the node should have an body (should be movable) + * - the position pointer param should point to a correct position struct * * post: - * - a physical node should be created in the world - * - the node properties should be set + * - if the node is physically availbe and is set to be movable + * the struct of the position pointer should be filled with + * the physical position of the node + * - otherwise the position should be set to zero */ - bool NodePhysics::createNode(NodeData* node) { + + bool ODEObject::createODEGeometry(interfaces::NodeData *node){ + std::cout << "ODEObject: using default createODEGeometry func. Did you forget to override it?." << std::endl; + LOG_WARN("ODEObject: using default createODEGeometry func. Did you forget to override it?."); + return true; + } + + bool ODEObject::changeNode(interfaces::NodeData* node) { + dReal pos[3] = {node->pos.x(), node->pos.y(), node->pos.z()}; + dQuaternion rotation; + rotation[1] = node->rot.x(); + rotation[2] = node->rot.y(); + rotation[3] = node->rot.z(); + rotation[0] = node->rot.w(); + const dReal *tpos; #ifdef _VERIFY_WORLD_ sRotation euler = utils::quaternionTosRotation(node->rot); fprintf(stderr, "node %d ; %.4f, %.4f, %.4f ; %.4f, %.4f, %.4f ; %.4f ; %.4f\n", @@ -147,37 +139,90 @@ namespace mars { node->mass, node->density); #endif MutexLocker locker(&(theWorld->iMutex)); - if(theWorld && theWorld->existsWorld()) { - bool ret; - //LOG_DEBUG("physicMode %d", node->physicMode); + + if(nGeom && theWorld && theWorld->existsWorld()) { + if(composite) { + dGeomGetQuaternion(nGeom, rotation); + tpos = dGeomGetPosition(nGeom); + pos[0] = tpos[0]; + pos[1] = tpos[1]; + pos[2] = tpos[2]; + } + // deferre destruction of geom until after the successful creation of + // a new geom + dGeomID tmpGeomId = nGeom; // first we create a ode geometry for the node - switch(node->physicMode) { - case NODE_TYPE_MESH: - ret = createMesh(node); - break; - case NODE_TYPE_BOX: - ret = createBox(node); - break; - case NODE_TYPE_SPHERE: - ret = createSphere(node); - break; - case NODE_TYPE_CAPSULE: - ret = createCapsule(node); - break; - case NODE_TYPE_CYLINDER: - ret = createCylinder(node); - break; - case NODE_TYPE_PLANE: - ret = createPlane(node); - break; - case NODE_TYPE_TERRAIN: - ret = createHeightfield(node); - break; - default: - // no correct type is spezified, so no physically node will be created + bool success = false; + success = createODEGeometry(node); + if(!success) { + fprintf(stderr, "creation of body geometry failed.\n"); return 0; - break; } + if(nBody) { + theWorld->destroyBody(nBody, this); + nBody = NULL; + } + dGeomDestroy(tmpGeomId); + // now the geom is rebuild and we have to reconnect it to the body + // and reset the mass of the body + if(!node->movable) { + dGeomSetBody(nGeom, nBody); + dGeomSetQuaternion(nGeom, rotation); + dGeomSetPosition(nGeom, (dReal)node->pos.x(), + (dReal)node->pos.y(), (dReal)node->pos.z()); + } + else { + bool body_created = false; + if(node->groupID) { + body_created = theWorld->getCompositeBody(node->groupID, &nBody, this); + composite = true; + } + else { + composite = false; + if(!nBody) nBody = dBodyCreate(theWorld->getWorld()); + } + if(nBody) { + dGeomSetBody(nGeom, nBody); + if(!composite) { + dBodySetMass(nBody, &nMass); + dGeomSetQuaternion(nGeom, rotation); + dGeomSetPosition(nGeom, pos[0], pos[1], pos[2]); + } + else { + // if the geom is part of a composite object + // we have to translate and rotate the geom mass + if(body_created) { + dBodySetMass(nBody, &nMass); + dBodySetPosition(nBody, pos[0], pos[1], pos[2]); + dBodySetQuaternion(nBody, rotation); + } + else { + dGeomSetOffsetWorldQuaternion(nGeom, rotation); + dGeomSetOffsetWorldPosition(nGeom, pos[0], pos[1], pos[2]); + theWorld->resetCompositeMass(nBody); + } + } + } + } + dGeomSetData(nGeom, &node_data); + locker.unlock(); + setContactParams(node->c_params); + } + return 1; + } + + bool ODEObject::createNode(interfaces::NodeData* node) { +#ifdef _VERIFY_WORLD_ + sRotation euler = utils::quaternionTosRotation(node->rot); + fprintf(stderr, "node %d ; %.4f, %.4f, %.4f ; %.4f, %.4f, %.4f ; %.4f ; %.4f\n", + node->index, node->pos.x(), node->pos.y(), + node->pos.z(), euler.alpha, euler.beta, euler.gamma, + node->mass, node->density); +#endif + MutexLocker locker(&(theWorld->iMutex)); + if(theWorld && theWorld->existsWorld()) { + bool ret; + ret = createODEGeometry(node); if(ret == 0) { // Error createing the physical Node return 0; @@ -186,15 +231,16 @@ namespace mars { // then, if the geometry was sucsessfully build, we can create a // body for the node or add the node to an existing body if(node->movable) setProperties(node); - else if(node->physicMode != NODE_TYPE_PLANE) { + else if(node->nodeType != "plane") { dQuaternion tmp, t1, t2; tmp[1] = (dReal)node->rot.x(); tmp[2] = (dReal)node->rot.y(); tmp[3] = (dReal)node->rot.z(); tmp[0] = (dReal)node->rot.w(); dGeomSetPosition(nGeom, (dReal)node->pos.x(), - (dReal)node->pos.y(), (dReal)node->pos.z()); - if(node->physicMode == NODE_TYPE_TERRAIN) { + (dReal)node->pos.y(), (dReal)node->pos.z()); + //TODO is this irrelevant as we are in ODEBox now? or is this still possible due to wrong parameter definition/ class selection + if(node->nodeType == "terrain") { dGeomGetQuaternion(nGeom, t1); dQMultiply0(t2, tmp, t1); dNormalize4(t2); @@ -219,28 +265,15 @@ namespace mars { dGeomSetData(nGeom, &node_data); locker.unlock(); setContactParams(node->c_params); + object_created = true; return 1; } - LOG_WARN("NodePhysics: tried to create a Node without there being a world."); + LOG_WARN("ODEBox: tried to create a Box without there being a world."); return 0; } - /** - * \brief The method copies the position of the node at the adress - * of the pointer pos. - * - * pre: - * - the physical representation of the node should be availbe - * - the node should have an body (should be movable) - * - the position pointer param should point to a correct position struct - * - * post: - * - if the node is physically availbe and is set to be movable - * the struct of the position pointer should be filled with - * the physical position of the node - * - otherwise the position should be set to zero - */ - void NodePhysics::getPosition(Vector* pos) const { + + void ODEObject::getPosition(Vector* pos) const { MutexLocker locker(&(theWorld->iMutex)); if(nBody) { const dReal* tmp = dGeomGetPosition(nGeom); @@ -286,7 +319,7 @@ namespace mars { * set the position of the corresponding body to the given parameter * - otherwise, we have to do nothing */ - const Vector NodePhysics::setPosition(const Vector &pos, bool move_group) { + const Vector ODEObject::setPosition(const Vector &pos, bool move_group) { const dReal *tpos; const dReal *tpos2; dReal npos[3]; @@ -368,7 +401,7 @@ namespace mars { * of the node * - otherwise a standard return of zero rotation should be set */ - void NodePhysics::getRotation(Quaternion* q) const { + void ODEObject::getRotation(Quaternion* q) const { dQuaternion tmp; MutexLocker locker(&(theWorld->iMutex)); @@ -402,7 +435,7 @@ namespace mars { * velocity of the node * - otherwise a standard return of zero velocity should be set */ - void NodePhysics::getLinearVelocity(Vector* vel) const { + void ODEObject::getLinearVelocity(Vector* vel) const { const dReal *tmp; MutexLocker locker(&(theWorld->iMutex)); @@ -434,7 +467,7 @@ namespace mars { * velocity of the node * - otherwise a standard return of zero velocity should be set */ - void NodePhysics::getAngularVelocity(Vector* vel) const { + void ODEObject::getAngularVelocity(Vector* vel) const { const dReal *tmp; MutexLocker locker(&(theWorld->iMutex)); @@ -466,7 +499,7 @@ namespace mars { * force of the node * - otherwise a standard return of zero force should be set */ - void NodePhysics::getForce(Vector* f) const { + void ODEObject::getForce(Vector* f) const { const dReal *tmp; MutexLocker locker(&(theWorld->iMutex)); @@ -498,7 +531,7 @@ namespace mars { * of the node * - otherwise a standard return of zero torque should be set */ - void NodePhysics::getTorque(Vector *t) const { + void ODEObject::getTorque(Vector *t) const { const dReal *tmp; MutexLocker locker(&(theWorld->iMutex)); @@ -523,7 +556,7 @@ namespace mars { * If we need it, the pre and post conditions are like them in the set * position method. */ - const Quaternion NodePhysics::setRotation(const Quaternion &q, bool move_group) { + const Quaternion ODEObject::setRotation(const Quaternion &q, bool move_group) { dQuaternion tmp, tmp2, tmp3, tmp4, tmp5; const dReal *brot, *bpos, *gpos; Quaternion q2; @@ -601,7 +634,7 @@ namespace mars { * * I don't think that we need this function. */ - void NodePhysics::setWorldObject(std::shared_ptr world) { + void ODEObject::setWorldObject(std::shared_ptr world) { theWorld = std::dynamic_pointer_cast(world); } @@ -611,272 +644,16 @@ namespace mars { * body from joint physics *TO DO : test if the Node has a body */ - dBodyID NodePhysics::getBody() const { + dBodyID ODEObject::getBody() const { return nBody; } - /** - * \brief The method creates an ode mesh representation of the given node. - * - * - */ - bool NodePhysics::createMesh(NodeData* node) { - int i; - - if (!node->inertia_set && - (node->ext.x() <= 0 || node->ext.y() <= 0 || node->ext.z() <= 0)) { - LOG_ERROR("Cannot create Node \"%s\" (id=%lu):\n" - " Mesh Nodes must have ext.x(), ext.y(), and ext.z() > 0.\n" - " Current values are: x=%g; y=%g, z=%g", - node->name.c_str(), node->index, - node->ext.x(), node->ext.y(), node->ext.z()); - return false; - } - - myVertices = (dVector3*)calloc(node->mesh.vertexcount, sizeof(dVector3)); - myIndices = (dTriIndex*)calloc(node->mesh.indexcount, sizeof(dTriIndex)); - //LOG_DEBUG("%d %d", node->mesh.vertexcount, node->mesh.indexcount); - // first we have to copy the mesh data to prevent errors in case - // of double to float conversion - dReal minx, miny, minz, maxx, maxy, maxz; - for(i=0; imesh.vertexcount; i++) { - myVertices[i][0] = (dReal)node->mesh.vertices[i][0]; - myVertices[i][1] = (dReal)node->mesh.vertices[i][1]; - myVertices[i][2] = (dReal)node->mesh.vertices[i][2]; - if(i==0) { - minx = myVertices[i][0]; - maxx = myVertices[i][0]; - miny = myVertices[i][1]; - maxy = myVertices[i][1]; - minz = myVertices[i][2]; - maxz = myVertices[i][2]; - } - else { - if(minx > myVertices[i][0]) minx = myVertices[i][0]; - if(maxx < myVertices[i][0]) maxx = myVertices[i][0]; - if(miny > myVertices[i][1]) miny = myVertices[i][1]; - if(maxy < myVertices[i][1]) maxy = myVertices[i][1]; - if(minz > myVertices[i][2]) minz = myVertices[i][2]; - if(maxz < myVertices[i][2]) maxz = myVertices[i][2]; - } - } - // rescale - dReal sx = node->ext.x()/(maxx-minx); - dReal sy = node->ext.y()/(maxy-miny); - dReal sz = node->ext.z()/(maxz-minz); - for(i=0; imesh.vertexcount; i++) { - myVertices[i][0] *= sx; - myVertices[i][1] *= sy; - myVertices[i][2] *= sz; - } - for(i=0; imesh.indexcount; i++) { - myIndices[i] = (dTriIndex)node->mesh.indices[i]; - } - - // then we can build the ode representation - myTriMeshData = dGeomTriMeshDataCreate(); - dGeomTriMeshDataBuildSimple(myTriMeshData, (dReal*)myVertices, - node->mesh.vertexcount, - myIndices, node->mesh.indexcount); - nGeom = dCreateTriMesh(theWorld->getSpace(), myTriMeshData, 0, 0, 0); - - // at this moment we set the mass properties as the mass of the - // bounding box if no mass and inertia is set by the user - if(node->inertia_set) { - setInertiaMass(node); - } - else if(node->density > 0) { - dMassSetBox(&nMass, (dReal)node->density, (dReal)node->ext.x(), - (dReal)node->ext.y(),(dReal)node->ext.z()); - } - else if(node->mass > 0) { - dMassSetBoxTotal(&nMass, (dReal)node->mass, (dReal)node->ext.x(), - (dReal)node->ext.y(),(dReal)node->ext.z()); - } - return true; - } - - /** - * The method creates an ode box representation of the given node. - * - */ - bool NodePhysics::createBox(NodeData* node) { - if (!node->inertia_set && - (node->ext.x() <= 0 || node->ext.y() <= 0 || node->ext.z() <= 0)) { - LOG_ERROR("Cannot create Node \"%s\" (id=%lu):\n" - " Box Nodes must have ext.x(), ext.y(), and ext.z() > 0.\n" - " Current values are: x=%g; y=%g, z=%g", - node->name.c_str(), node->index, - node->ext.x(), node->ext.y(), node->ext.z()); - return false; - } - - // build the ode representation - nGeom = dCreateBox(theWorld->getSpace(), (dReal)(node->ext.x()), - (dReal)(node->ext.y()), (dReal)(node->ext.z())); - - // create the mass object for the box - if(node->inertia_set) { - setInertiaMass(node); - } - else if(node->density > 0) { - dMassSetBox(&nMass, (dReal)(node->density), (dReal)(node->ext.x()), - (dReal)(node->ext.y()),(dReal)(node->ext.z())); - } - else if(node->mass > 0) { - dReal tempMass =(dReal)(node->mass); - dMassSetBoxTotal(&nMass, tempMass, (dReal)(node->ext.x()), - (dReal)(node->ext.y()),(dReal)(node->ext.z())); - } - return true; - } - - /** - * The method creates an ode shpere representation of the given node. - * - */ - bool NodePhysics::createSphere(NodeData* node) { - if (!node->inertia_set && node->ext.x() <= 0) { - LOG_ERROR("Cannot create Node \"%s\" (id=%lu):\n" - " Sphere Nodes must have ext.x() > 0.\n" - " Current value is: x=%g", - node->name.c_str(), node->index, node->ext.x()); - return false; - } - - // build the ode representation - nGeom = dCreateSphere(theWorld->getSpace(), (dReal)node->ext.x()); - - // create the mass object for the sphere - if(node->inertia_set) { - setInertiaMass(node); - } - else if(node->density > 0) { - dMassSetSphere(&nMass, (dReal)node->density, (dReal)node->ext.x()); - } - else if(node->mass > 0) { - dMassSetSphereTotal(&nMass, (dReal)node->mass, (dReal)node->ext.x()); - } - return true; - } - - /** - * The method creates an ode capsule representation of the given node. - * - */ - bool NodePhysics::createCapsule(NodeData* node) { - if (!node->inertia_set && (node->ext.x() <= 0 || node->ext.y() <= 0)) { - LOG_ERROR("Cannot create Node \"%s\" (id=%lu):\n" - " Capsule Nodes must have ext.x() and ext.y() > 0.\n" - " Current values are: x=%g; y=%g", - node->name.c_str(), node->index, - node->ext.x(), node->ext.y()); - return false; - } - - // build the ode representation - nGeom = dCreateCapsule(theWorld->getSpace(), (dReal)node->ext.x(), - (dReal)node->ext.y()); - - // create the mass object for the capsule - if(node->inertia_set) { - setInertiaMass(node); - } - else if(node->density > 0) { - dMassSetCapsule(&nMass, (dReal)node->density, 3, (dReal)node->ext.x(), - (dReal)node->ext.y()); - } - else if(node->mass > 0) { - dMassSetCapsuleTotal(&nMass, (dReal)node->mass, 3, (dReal)node->ext.x(), - (dReal)node->ext.y()); - } - return true; - } - - /** - * The method creates an ode cylinder representation of the given node. - * - */ - bool NodePhysics::createCylinder(NodeData* node) { - if (!node->inertia_set && (node->ext.x() <= 0 || node->ext.y() <= 0)) { - LOG_ERROR("Cannot create Node \"%s\" (id=%lu):\n" - " Cylinder Nodes must have ext.x() and ext.y() > 0.\n" - " Current values are: x=%g; y=%g", - node->name.c_str(), node->index, - node->ext.x(), node->ext.y()); - return false; - } - - // build the ode representation - nGeom = dCreateCylinder(theWorld->getSpace(), (dReal)node->ext.x(), - (dReal)node->ext.y()); - - // create the mass object for the cylinder - if(node->inertia_set) { - setInertiaMass(node); - } - else if(node->density > 0) { - dMassSetCylinder(&nMass, (dReal)node->density, 3, (dReal)node->ext.x(), - (dReal)node->ext.y()); - } - else if(node->mass > 0) { - dMassSetCylinderTotal(&nMass, (dReal)node->mass, 3, (dReal)node->ext.x(), - (dReal)node->ext.y()); - } - return true; - } - - /** - * The method creates an ode plane - * - */ - bool NodePhysics::createPlane(NodeData* node) { - - // build the ode representation - nGeom = dCreatePlane(theWorld->getSpace(), 0, 0, 1, (dReal)node->pos.z()); - return true; - } - - bool NodePhysics::createHeightfield(NodeData* node) { - dMatrix3 R; - unsigned long size; - int x, y; - terrain = node->terrain; - size = terrain->width*terrain->height; - if(!height_data) height_data = (dReal*)calloc(size, sizeof(dReal)); - for(x=0; xheight; x++) { - for(y=0; ywidth; y++) { - height_data[(terrain->height-(x+1))*terrain->width+y] = (dReal)terrain->pixelData[x*terrain->width+y]; - } - } - // build the ode representation - dHeightfieldDataID heightid = dGeomHeightfieldDataCreate(); - - // Create an finite heightfield. - dGeomHeightfieldDataBuildCallback(heightid, this, heightfield_callback, - terrain->targetWidth, - terrain->targetHeight, - terrain->width, terrain->height, - REAL(1.0), REAL( 0.0 ), - REAL(1.0), 0); - // Give some very bounds which, while conservative, - // makes AABB computation more accurate than +/-INF. - dGeomHeightfieldDataSetBounds(heightid, REAL(-terrain->scale*2.0), - REAL(terrain->scale*2.0)); - //dGeomHeightfieldDataSetBounds(heightid, -terrain->scale, terrain->scale); - nGeom = dCreateHeightfield(theWorld->getSpace(), heightid, 1); - dRSetIdentity(R); - dRFromAxisAndAngle(R, 1, 0, 0, M_PI/2); - dGeomSetRotation(nGeom, R); - return true; - } - /** * This method sets some properties for the node. The properties includes * the posistion, the rotation, the movability and the coposite group number * */ - void NodePhysics::setProperties(NodeData* node) { + void ODEObject::setProperties(NodeData* node) { bool body_created = 1; dQuaternion tmp; @@ -937,7 +714,7 @@ namespace mars { * * post: */ - const Vector NodePhysics::rotateAtPoint(const Vector &rotation_point, + const Vector ODEObject::rotateAtPoint(const Vector &rotation_point, const Quaternion &rotation, bool move_group) { dQuaternion tmp, tmp2, tmp3; @@ -1021,128 +798,6 @@ namespace mars { return npos; } - /** - * \brief This function rebuilds the geom (type, size and mass) of a node - * - * - * pre: - * - the world should be exist - * - a geom should be exist - * - * post: - * - the geom should be rebuild with the new properties - */ - bool NodePhysics::changeNode(NodeData* node) { - dReal pos[3] = {node->pos.x(), node->pos.y(), node->pos.z()}; - dQuaternion rotation; - rotation[1] = node->rot.x(); - rotation[2] = node->rot.y(); - rotation[3] = node->rot.z(); - rotation[0] = node->rot.w(); - const dReal *tpos; -#ifdef _VERIFY_WORLD_ - sRotation euler = utils::quaternionTosRotation(node->rot); - fprintf(stderr, "node %d ; %.4f, %.4f, %.4f ; %.4f, %.4f, %.4f ; %.4f ; %.4f\n", - node->index, node->pos.x(), node->pos.y(), - node->pos.z(), euler.alpha, euler.beta, euler.gamma, - node->mass, node->density); -#endif - MutexLocker locker(&(theWorld->iMutex)); - - if(nGeom && theWorld && theWorld->existsWorld()) { - if(composite) { - dGeomGetQuaternion(nGeom, rotation); - tpos = dGeomGetPosition(nGeom); - pos[0] = tpos[0]; - pos[1] = tpos[1]; - pos[2] = tpos[2]; - } - // deferre destruction of geom until after the successful creation of - // a new geom - dGeomID tmpGeomId = nGeom; - // first we create a ode geometry for the node - bool success = false; - switch(node->physicMode) { - case NODE_TYPE_MESH: - success = createMesh(node); - break; - case NODE_TYPE_BOX: - success = createBox(node); - break; - case NODE_TYPE_SPHERE: - success = createSphere(node); - break; - case NODE_TYPE_CAPSULE: - success = createCapsule(node); - break; - case NODE_TYPE_CYLINDER: - success = createCylinder(node); - break; - case NODE_TYPE_PLANE: - success = createPlane(node); - break; - default: - // no correct type is spezified, so no physically node will be created - success = false; - break; - } - if(!success) { - fprintf(stderr, "creation of body geometry failed.\n"); - return 0; - } - if(nBody) { - theWorld->destroyBody(nBody, this); - nBody = NULL; - } - dGeomDestroy(tmpGeomId); - // now the geom is rebuild and we have to reconnect it to the body - // and reset the mass of the body - if(!node->movable) { - dGeomSetBody(nGeom, nBody); - dGeomSetQuaternion(nGeom, rotation); - dGeomSetPosition(nGeom, (dReal)node->pos.x(), - (dReal)node->pos.y(), (dReal)node->pos.z()); - } - else { - bool body_created = false; - if(node->groupID) { - body_created = theWorld->getCompositeBody(node->groupID, &nBody, this); - composite = true; - } - else { - composite = false; - if(!nBody) nBody = dBodyCreate(theWorld->getWorld()); - } - if(nBody) { - dGeomSetBody(nGeom, nBody); - if(!composite) { - dBodySetMass(nBody, &nMass); - dGeomSetQuaternion(nGeom, rotation); - dGeomSetPosition(nGeom, pos[0], pos[1], pos[2]); - } - else { - // if the geom is part of a composite object - // we have to translate and rotate the geom mass - if(body_created) { - dBodySetMass(nBody, &nMass); - dBodySetPosition(nBody, pos[0], pos[1], pos[2]); - dBodySetQuaternion(nBody, rotation); - } - else { - dGeomSetOffsetWorldQuaternion(nGeom, rotation); - dGeomSetOffsetWorldPosition(nGeom, pos[0], pos[1], pos[2]); - theWorld->resetCompositeMass(nBody); - } - } - } - } - dGeomSetData(nGeom, &node_data); - locker.unlock(); - setContactParams(node->c_params); - } - return 1; - } - /** * \brief returns the ode mass object * @@ -1151,7 +806,7 @@ namespace mars { * * post: */ - dMass NodePhysics::getODEMass(void) const { + dMass ODEObject::getODEMass(void) const { return nMass; } @@ -1164,7 +819,7 @@ namespace mars { * post: * - the linear velocity of the body should be set */ - void NodePhysics::setLinearVelocity(const Vector &velocity) { + void ODEObject::setLinearVelocity(const Vector &velocity) { MutexLocker locker(&(theWorld->iMutex)); if(nBody) dBodySetLinearVel(nBody, (dReal)velocity.x(), (dReal)velocity.y(), (dReal)velocity.z()); @@ -1179,7 +834,7 @@ namespace mars { * post: * - the angular velocity of the body should be set */ - void NodePhysics::setAngularVelocity(const Vector &velocity) { + void ODEObject::setAngularVelocity(const Vector &velocity) { MutexLocker locker(&(theWorld->iMutex)); if(nBody) dBodySetAngularVel(nBody, (dReal)velocity.x(), (dReal)velocity.y(), (dReal)velocity.z()); @@ -1194,7 +849,7 @@ namespace mars { * post: * - the force of the body should be set */ - void NodePhysics::setForce(const Vector &f) { + void ODEObject::setForce(const Vector &f) { MutexLocker locker(&(theWorld->iMutex)); if(nBody) dBodySetForce(nBody, (dReal)f.x(), (dReal)f.y(), (dReal)f.z()); @@ -1209,7 +864,7 @@ namespace mars { * post: * - the torque of the body should be set */ - void NodePhysics::setTorque(const Vector &t) { + void ODEObject::setTorque(const Vector &t) { MutexLocker locker(&(theWorld->iMutex)); if(nBody) dBodySetTorque(nBody, (dReal)t.x(), (dReal)t.y(), (dReal)t.z()); @@ -1224,7 +879,7 @@ namespace mars { * post: * - the force should be added to the body */ - void NodePhysics::addForce(const Vector &f, const Vector &p) { + void ODEObject::addForce(const Vector &f, const Vector &p) { MutexLocker locker(&(theWorld->iMutex)); if(nBody) { dBodyAddForceAtPos(nBody, @@ -1241,7 +896,7 @@ namespace mars { * post: * - the force should be added to the body */ - void NodePhysics::addForce(const Vector &f) { + void ODEObject::addForce(const Vector &f) { MutexLocker locker(&(theWorld->iMutex)); if(nBody) { dBodyAddForce(nBody, (dReal)f.x(), (dReal)f.y(), (dReal)f.z()); @@ -1257,19 +912,19 @@ namespace mars { * post: * - the torque should be added to the body */ - void NodePhysics::addTorque(const Vector &t) { + void ODEObject::addTorque(const Vector &t) { MutexLocker locker(&(theWorld->iMutex)); if(nBody) dBodyAddTorque(nBody, (dReal)t.x(), (dReal)t.y(), (dReal)t.z()); } - bool NodePhysics::getGroundContact(void) const { + bool ODEObject::getGroundContact(void) const { if(nGeom) { return node_data.num_ground_collisions; } return false; } - void NodePhysics::getContactPoints(std::vector *contact_points) const { + void ODEObject::getContactPoints(std::vector *contact_points) const { contact_points->clear(); if(nGeom) { std::vector::const_iterator iter; @@ -1281,7 +936,7 @@ namespace mars { } } - void NodePhysics::getContactIDs(std::list *ids) const { + void ODEObject::getContactIDs(std::list *ids) const { ids->clear(); if(nGeom) { *ids = node_data.contact_ids; @@ -1289,7 +944,7 @@ namespace mars { } - sReal NodePhysics::getGroundContactForce(void) const { + sReal ODEObject::getGroundContactForce(void) const { std::vector::const_iterator iter; dReal force[3] = {0,0,0}; @@ -1311,7 +966,7 @@ namespace mars { return dLENGTH(force); } - const Vector NodePhysics::getContactForce(void) const { + const Vector ODEObject::getContactForce(void) const { std::vector::const_iterator iter; dReal force[3] = {0,0,0}; @@ -1333,7 +988,7 @@ namespace mars { return Vector(force[0], force[1], force[2]); } - void NodePhysics::addCompositeOffset(dReal x, dReal y, dReal z) { + void ODEObject::addCompositeOffset(dReal x, dReal y, dReal z) { // no lock because physics internal functions get locked elsewhere const dReal *gpos; @@ -1341,7 +996,7 @@ namespace mars { dGeomSetOffsetWorldPosition(nGeom, gpos[0]+x, gpos[1]+y, gpos[2]+z); } - void NodePhysics::addMassToCompositeBody(dBodyID theBody, dMass *bodyMass) { + void ODEObject::addMassToCompositeBody(dBodyID theBody, dMass *bodyMass) { // no lock because physics internal functions get locked elsewhere const dReal *rotation, *pos, *brot; dReal tpos[3]; @@ -1367,7 +1022,7 @@ namespace mars { dMassTranslate(bodyMass, -bodyMass->c[0], -bodyMass->c[1], -bodyMass->c[2]); } - void NodePhysics::getAbsMass(dMass *tMass) const { + void ODEObject::getAbsMass(dMass *tMass) const { // no lock because physics internal functions get locked elsewhere const dReal *pos = dGeomGetPosition(nGeom); const dReal *rot = dGeomGetRotation(nGeom); @@ -1377,12 +1032,7 @@ namespace mars { dMassTranslate(tMass, pos[0], pos[1], pos[2]); } - dReal NodePhysics::heightCallback(int x, int y) { - - return (dReal)height_data[(y*terrain->width)+x]*terrain->scale; - } - - void NodePhysics::setContactParams(contact_params& c_params) { + void ODEObject::setContactParams(contact_params& c_params) { MutexLocker locker(&(theWorld->iMutex)); node_data.c_params = c_params; if(nGeom) { @@ -1406,7 +1056,7 @@ namespace mars { * - the memory for the sensor data has to be allocated * - the physical elements for the sensor had to be created */ - void NodePhysics::addSensor(BaseSensor* sensor) { + void ODEObject::addSensor(BaseSensor* sensor) { MutexLocker locker(&(theWorld->iMutex)); int i; geom_data* gd; @@ -1587,7 +1237,7 @@ namespace mars { } } - void NodePhysics::removeSensor(BaseSensor *sensor) { + void ODEObject::removeSensor(BaseSensor *sensor) { MutexLocker locker(&(theWorld->iMutex)); std::vector::iterator iter; for (iter = sensor_list.begin(); iter != sensor_list.end(); ) { @@ -1608,7 +1258,7 @@ namespace mars { * * post: */ - void NodePhysics::handleSensorData(bool physics_thread) { + void ODEObject::handleSensorData(bool physics_thread) { if(!physics_thread) return; MutexLocker locker(&(theWorld->iMutex)); std::vector::iterator iter; @@ -1735,28 +1385,19 @@ namespace mars { * * post: */ - void NodePhysics::destroyNode(void) { + void ODEObject::destroyNode(void) { MutexLocker locker(&(theWorld->iMutex)); if(nBody) theWorld->destroyBody(nBody, this); - if(nGeom) dGeomDestroy(nGeom); - - if(myVertices) free(myVertices); - if(myIndices) free(myIndices); - if(myTriMeshData) dGeomTriMeshDataDestroy(myTriMeshData); - nBody = 0; nGeom = 0; - myVertices = 0; - myIndices = 0; - myTriMeshData = 0; composite = false; //node_data.num_ground_collisions = 0; node_data.setZero(); - height_data = 0; + std::cout << "Destroyed Parent" << std::endl; } - void NodePhysics::setInertiaMass(NodeData* node) { + void ODEObject::setInertiaMass(NodeData* node) { dMassSetZero(&nMass); nMass.mass = (dReal)node->mass; @@ -1776,7 +1417,7 @@ namespace mars { nMass.I[11] = 0.0; } - void NodePhysics::getMass(sReal *mass, sReal *inertia) const { + void ODEObject::getMass(sReal *mass, sReal *inertia) const { if(mass) *mass = nMass.mass; if(inertia) { inertia[0] = nMass.I[0]; @@ -1791,7 +1432,7 @@ namespace mars { } } - std::vector NodePhysics::addContacts(ContactsPhysics contacts, dWorldID world, dJointGroupID contactgroup){ + std::vector ODEObject::addContacts(ContactsPhysics contacts, dWorldID world, dJointGroupID contactgroup){ dVector3 v; //dMatrix3 R; dReal dot; @@ -1822,8 +1463,8 @@ namespace mars { // Otherwise use a parameter for this if(contactsPtr->operator[](i).geom.depth > 0.001) { - LOG_DEBUG("[NodePhysics::createContacts]:The colision detected might have too large depth, reducing it to the maxmimum allowed"); - std::cout << "[NodePhysics::createContacts]: Depth " << contactsPtr->operator[](i).geom.depth << std::endl; + LOG_DEBUG("[ODEObject::createContacts]:The colision detected might have too large depth, reducing it to the maxmimum allowed"); + std::cout << "[ODEObject::createContacts]: Depth " << contactsPtr->operator[](i).geom.depth << std::endl; contactsPtr->operator[](i).geom.depth = 0.001; } */ @@ -1843,7 +1484,7 @@ namespace mars { { LOG_WARN("Catched a nan in the feedback forces, won't be used"); } - LOG_DEBUG("[NodePhysics::addContacts] Contacts were added to the node %s", nodeName.c_str()); + LOG_DEBUG("[ODEObject::addContacts] Contacts were added to the node %s", nodeName.c_str()); #endif fbs.push_back(fb); addContact(c, contactsPtr->operator[](i), fb); @@ -1851,7 +1492,7 @@ namespace mars { return fbs; } - void NodePhysics::addContact(dJointID contactJointId, dContact contact, dJointFeedback* fb){ + void ODEObject::addContact(dJointID contactJointId, dContact contact, dJointFeedback* fb){ Vector contact_point; dJointAttach(contactJointId, nBody, 0); node_data.num_ground_collisions ++; @@ -1865,12 +1506,12 @@ namespace mars { } - sReal NodePhysics::getCollisionDepth(void) const { + sReal ODEObject::getCollisionDepth(void) const { if(nGeom && theWorld) { return theWorld->getCollisionDepth(nGeom); } return 0.0; } - } // end of namespace sim +} // end of namespace sim } // end of namespace mars diff --git a/sim/src/physics/NodePhysics.h b/sim/src/physics/objects/ODEObject.h similarity index 82% rename from sim/src/physics/NodePhysics.h rename to sim/src/physics/objects/ODEObject.h index e876bc162..cf13e089f 100644 --- a/sim/src/physics/NodePhysics.h +++ b/sim/src/physics/objects/ODEObject.h @@ -1,5 +1,5 @@ /* - * Copyright 2011, 2012, DFKI GmbH Robotics Innovation Center + * Copyright 2022, DFKI GmbH Robotics Innovation Center * * This file is part of the MARS simulation framework. * @@ -19,17 +19,17 @@ */ /** - * \file NodePhysics.h - * \author Malte Roemmermann - * \brief "NodePhysics" implements the physical ode stuff for the nodes. + * \file ODEObject.h + * \author Malte Roemmermann, Leon Danter, Muhammad Haider Khan Lodhi + * \brief "ODEObject" implements an ODEObject as parent of ode objects. * */ -#ifndef NODE_PHYSICS_H -#define NODE_PHYSICS_H +#ifndef ODE_OBJECT_H +#define ODE_OBJECT_H #ifdef _PRINT_HEADER_ - #warning "NodePhysics.h" + #warning "ODEObject.h" #endif #include "WorldPhysics.h" @@ -40,8 +40,9 @@ #define dTriIndex int #endif +//TODO move struct descriptions to seperate file! namespace mars { - namespace sim { +namespace sim { /* * we need a data structure to handle different collision parameter @@ -93,11 +94,15 @@ namespace mars { * The class that implements the NodeInterface interface. * */ - class NodePhysics : public interfaces::NodeInterface { + + class ODEObject : public interfaces::NodeInterface { public: - NodePhysics(std::shared_ptr world); - virtual ~NodePhysics(void); - virtual bool createNode(interfaces::NodeData *node); + ODEObject(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEObject(void); + + virtual bool createNode(interfaces::NodeData *node) override; + virtual bool changeNode(interfaces::NodeData *node) override; + virtual bool createODEGeometry(interfaces::NodeData *node); virtual void getPosition(utils::Vector *pos) const; virtual const utils::Vector setPosition(const utils::Vector &pos, bool move_group); virtual void getRotation(utils::Quaternion *q) const; @@ -110,7 +115,6 @@ namespace mars { virtual const utils::Vector rotateAtPoint(const utils::Vector &rotation_point, const utils::Quaternion &rotation, bool move_group); - virtual bool changeNode(interfaces::NodeData *node); virtual void setLinearVelocity(const utils::Vector &velocity); virtual void setAngularVelocity(const utils::Vector &velocity); virtual void setForce(const utils::Vector &f); @@ -139,33 +143,24 @@ namespace mars { dMass getODEMass(void) const; void addMassToCompositeBody(dBodyID theBody, dMass *bodyMass); void getAbsMass(dMass *pMass) const; - dReal heightCallback(int x, int y); + bool isObjectCreated(); protected: std::shared_ptr theWorld; dBodyID nBody; dGeomID nGeom; dMass nMass; - dVector3 *myVertices; - dTriIndex *myIndices; - dTriMeshDataID myTriMeshData; bool composite; geom_data node_data; - interfaces::terrainStruct *terrain; - dReal *height_data; std::vector sensor_list; - bool createMesh(interfaces::NodeData *node); - bool createBox(interfaces::NodeData *node); - bool createSphere(interfaces::NodeData *node); - bool createCapsule(interfaces::NodeData *node); - bool createCylinder(interfaces::NodeData *node); - bool createPlane(interfaces::NodeData *node); - bool createHeightfield(interfaces::NodeData *node); void setProperties(interfaces::NodeData *node); void setInertiaMass(interfaces::NodeData *node); + + private: + bool object_created; }; - } // end of namespace sim +} // end of namespace sim } // end of namespace mars #endif // NODE_PHYSICS_H diff --git a/sim/src/physics/objects/ODEObjectFactory.cpp b/sim/src/physics/objects/ODEObjectFactory.cpp new file mode 100644 index 000000000..42b1613a4 --- /dev/null +++ b/sim/src/physics/objects/ODEObjectFactory.cpp @@ -0,0 +1,70 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "objects/ODEObjectFactory.h" +#include "objects/ODEBox.h" +#include "objects/ODECapsule.h" +#include "objects/ODECylinder.h" +#include "objects/ODEMesh.h" +#include "objects/ODEPlane.h" +#include "objects/ODESphere.h" +#include "objects/ODEHeightField.h" + +#include + +namespace mars{ +namespace sim{ + +using namespace ::mars::interfaces; + +ODEObjectFactory& ODEObjectFactory::Instance(){ + static ODEObjectFactory instance; + return instance; +} + +ODEObjectFactory::ODEObjectFactory(){ +} + +ODEObjectFactory::~ODEObjectFactory(){ +} + +std::shared_ptr ODEObjectFactory::createObject(std::shared_ptr worldPhysics, NodeData * nodeData){ + + std::map::iterator it = availableObjects.find(nodeData->nodeType); + if(it == availableObjects.end()){ + throw std::runtime_error("Could not load unknown Physics Object with name: \"" + nodeData->name + "\"" ); + } + + std::shared_ptr newObject = std::make_shared(*(it->second(worldPhysics, nodeData))); + if(newObject->isObjectCreated()){ + return newObject; + } + else{ + std::cerr << "Failed to create Physics Object with name: \"" + nodeData->name + "\"" << std::endl; + return std::shared_ptr(nullptr); + } +} + +void ODEObjectFactory::addObjectType(const std::string& type, instantiateObjectfPtr funcPtr){ + availableObjects.insert(std::pair(type,funcPtr)); +} + +} +} diff --git a/sim/src/physics/objects/ODEObjectFactory.h b/sim/src/physics/objects/ODEObjectFactory.h new file mode 100644 index 000000000..64af008e1 --- /dev/null +++ b/sim/src/physics/objects/ODEObjectFactory.h @@ -0,0 +1,54 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODEObjectFactory.h + * \author Muhammad Haider Khan Lodhi + * \brief "ODEObjectFactory" implements ObjectFactoryInterface interface to create physical ode objects for the nodes. + * + */ + +#pragma once + +#include "objects/ObjectFactoryInterface.h" +#include "objects/ODEObject.h" + +namespace mars{ +namespace sim{ + +using namespace ::mars::interfaces; + +typedef ODEObject* (*instantiateObjectfPtr)(std::shared_ptr, NodeData *); + +class ODEObjectFactory : public ObjectFactoryInterface{ +public: + static ODEObjectFactory& Instance(); + virtual std::shared_ptr createObject(std::shared_ptr worldPhysics, NodeData * nodeData) override; + void addObjectType(const std::string& type, instantiateObjectfPtr funcPtr); + +private: + ODEObjectFactory(); + virtual ~ODEObjectFactory(); + std::map availableObjects; + +}; + +} +} diff --git a/sim/src/physics/objects/ODEPlane.cpp b/sim/src/physics/objects/ODEPlane.cpp new file mode 100644 index 000000000..1591596c4 --- /dev/null +++ b/sim/src/physics/objects/ODEPlane.cpp @@ -0,0 +1,48 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODEPlane.h" +#include + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEPlane::ODEPlane(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + createNode(nodeData); + } + + ODEPlane::~ODEPlane(void) { + } + + ODEObject* ODEPlane::instanciate(std::shared_ptr world, interfaces::NodeData * nodeData){ + return new ODEPlane(world, nodeData); + } + + bool ODEPlane::createODEGeometry(interfaces::NodeData *node){ + // build the ode representation + nGeom = dCreatePlane(theWorld->getSpace(), 0, 0, 1, (dReal)node->pos.z()); + return true; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/objects/ODEPlane.h b/sim/src/physics/objects/ODEPlane.h new file mode 100644 index 000000000..2bcf98c1b --- /dev/null +++ b/sim/src/physics/objects/ODEPlane.h @@ -0,0 +1,57 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODEPlane.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_PLANE_H +#define ODE_PLANE_H + +#ifdef _PRINT_HEADER_ + #warning "ODEPlane.h" +#endif + +#include +#include "ODEObject.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODEPlane : public ODEObject { + public: + ODEPlane(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEPlane(void); + static ODEObject* instanciate(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual bool createODEGeometry(interfaces::NodeData *node) override; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_PLANE_H diff --git a/sim/src/physics/objects/ODESphere.cpp b/sim/src/physics/objects/ODESphere.cpp new file mode 100644 index 000000000..013c612ea --- /dev/null +++ b/sim/src/physics/objects/ODESphere.cpp @@ -0,0 +1,71 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + +#include "ODESphere.h" +#include + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODESphere::ODESphere(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + createNode(nodeData); + } + + ODESphere::~ODESphere(void) { + } + + ODEObject* ODESphere::instanciate(std::shared_ptr world, interfaces::NodeData * nodeData){ + return new ODESphere(world, nodeData); + } + + /** + * The method creates an ode shpere representation of the given node. + * + */ + bool ODESphere::createODEGeometry(NodeData* node) { + if (!node->inertia_set && node->ext.x() <= 0) { + LOG_ERROR("Cannot create Node \"%s\" (id=%lu):\n" + " Sphere Nodes must have ext.x() > 0.\n" + " Current value is: x=%g", + node->name.c_str(), node->index, node->ext.x()); + return false; + } + + // build the ode representation + nGeom = dCreateSphere(theWorld->getSpace(), (dReal)node->ext.x()); + + // create the mass object for the sphere + if(node->inertia_set) { + setInertiaMass(node); + } + else if(node->density > 0) { + dMassSetSphere(&nMass, (dReal)node->density, (dReal)node->ext.x()); + } + else if(node->mass > 0) { + dMassSetSphereTotal(&nMass, (dReal)node->mass, (dReal)node->ext.x()); + } + return true; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/objects/ODESphere.h b/sim/src/physics/objects/ODESphere.h new file mode 100644 index 000000000..c17d407a2 --- /dev/null +++ b/sim/src/physics/objects/ODESphere.h @@ -0,0 +1,57 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ODESphere.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * + */ + +#ifndef ODE_SPHERE_H +#define ODE_SPHERE_H + +#ifdef _PRINT_HEADER_ + #warning "ODESphere.h" +#endif + +#include +#include "ODEObject.h" +#include + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { +namespace sim { + + class ODESphere : public ODEObject { + public: + ODESphere(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODESphere(void); + static ODEObject* instanciate(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual bool createODEGeometry(interfaces::NodeData *node) override; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_SPHERE_H diff --git a/sim/src/physics/objects/ObjectFactoryInterface.h b/sim/src/physics/objects/ObjectFactoryInterface.h new file mode 100644 index 000000000..157798674 --- /dev/null +++ b/sim/src/physics/objects/ObjectFactoryInterface.h @@ -0,0 +1,42 @@ +/* + * Copyright 2022, DFKI GmbH Robotics Innovation Center + * + * This file is part of the MARS simulation framework. + * + * MARS is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License + * as published by the Free Software Foundation, either version 3 + * of the License, or (at your option) any later version. + * + * MARS is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with MARS. If not, see . + * + */ + + /** + * \file ObjectFactoryInterface.h + * \author Muhammad Haider Khan Lodhi + * \brief "ObjectFactoryInterface" is an interface to create physical ode objects for the nodes. + * + */ + +#pragma once + +#include + +namespace mars{ +namespace sim{ + +using namespace ::mars::interfaces; + +class ObjectFactoryInterface{ +public: + virtual std::shared_ptr createObject(std::shared_ptr worldPhysics, NodeData * nodeData) = 0; +}; +} +} \ No newline at end of file diff --git a/sim/src/sensors/HapticFieldSensor.cpp b/sim/src/sensors/HapticFieldSensor.cpp index 93c76e804..84a97f6a5 100644 --- a/sim/src/sensors/HapticFieldSensor.cpp +++ b/sim/src/sensors/HapticFieldSensor.cpp @@ -71,7 +71,7 @@ namespace mars { forces.resize(config.cols*config.rows, 0.0); weights.resize(config.cols*config.rows, 1.0); - //control->nodes->addNodeSensor(this); //register sensor with NodePhysics + //control->nodes->addNodeSensor(this); //register sensor with ODEObject // register with DataBroker std::string groupName, dataName; diff --git a/sim/src/sensors/ScanningSonar.cpp b/sim/src/sensors/ScanningSonar.cpp index 9de0e2e91..183c09c29 100644 --- a/sim/src/sensors/ScanningSonar.cpp +++ b/sim/src/sensors/ScanningSonar.cpp @@ -82,7 +82,7 @@ namespace mars { std::stringstream s; s.str(""); s << config.name << "_fixed_joint"; NodeData ns_fixed( s.str(),position,orientation); - ns_fixed.initPrimitive(NODE_TYPE_CYLINDER,config.extension,0); + ns_fixed.initPrimitive("cylinder",config.extension,0); ns_fixed.movable = true; ns_fixed.noPhysical = false; ns_fixed.c_params.coll_bitmask = 0; @@ -90,7 +90,7 @@ namespace mars { s.str(""); s << config.name << "_play_joint"; NodeData ns_play(s.str(), config.position + (orientation * Vector(0, 0, config.extension[2])), orientation); - ns_play.initPrimitive(NODE_TYPE_CYLINDER,config.extension,0); + ns_play.initPrimitive("cylinder",config.extension,0); ns_play.movable = true; ns_play.noPhysical = false; ns_play.c_params.coll_bitmask = 0;