From edc6b2ffae769b2c1266c1bda208eef8dafab8bc Mon Sep 17 00:00:00 2001 From: Image Builder Date: Thu, 5 May 2022 20:20:15 +0000 Subject: [PATCH 01/21] introduce storage manager interface --- interfaces/src/sim/ControlCenter.h | 3 ++ interfaces/src/sim/StorageManagerInterface.h | 54 ++++++++++++++++++++ sim/src/core/Simulator.cpp | 5 ++ 3 files changed, 62 insertions(+) create mode 100644 interfaces/src/sim/StorageManagerInterface.h 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/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/sim/src/core/Simulator.cpp b/sim/src/core/Simulator.cpp index 698349a89..f73981f3b 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); From 99ddadd53949ae046e8de2bb536c80757215c374 Mon Sep 17 00:00:00 2001 From: Leon Cedric Danter Date: Tue, 2 Aug 2022 18:23:44 +0200 Subject: [PATCH 02/21] ODEBox is working. Code not clean. --- sim/CMakeLists.txt | 10 +- sim/src/core/NodeManager.cpp | 7 +- sim/src/core/PhysicsMapper.cpp | 60 +- sim/src/core/PhysicsMapper.h | 7 +- sim/src/physics/JointPhysics.cpp | 6 +- sim/src/physics/ODEBox.cpp | 460 ++++++++ sim/src/physics/ODEBox.h | 65 ++ sim/src/physics/ODEObject.cpp | 1384 +++++++++++++++++++++++++ sim/src/physics/ODEObject.h | 175 ++++ sim/src/physics/WorldPhysics.cpp | 16 +- sim/src/physics/WorldPhysics.h | 8 +- sim/src/physics/notes.md | 25 + sim/src/sensors/HapticFieldSensor.cpp | 2 +- 13 files changed, 2199 insertions(+), 26 deletions(-) create mode 100644 sim/src/physics/ODEBox.cpp create mode 100644 sim/src/physics/ODEBox.h create mode 100644 sim/src/physics/ODEObject.cpp create mode 100644 sim/src/physics/ODEObject.h create mode 100644 sim/src/physics/notes.md diff --git a/sim/CMakeLists.txt b/sim/CMakeLists.txt index 770552876..500ace94c 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) @@ -85,7 +87,9 @@ set(SOURCES_H src/sensors/RotatingRaySensor.h src/physics/JointPhysics.h - src/physics/NodePhysics.h + src/physics/ODEObject.h + src/physics/ODEBox.h +# src/physics/NodePhysics.h src/physics/WorldPhysics.h src/physics/ContactsPhysics.hpp @@ -137,7 +141,9 @@ set(TARGET_SRC src/sensors/RotatingRaySensor.cpp src/physics/JointPhysics.cpp - src/physics/NodePhysics.cpp + # src/physics/NodePhysics.cpp + src/physics/ODEObject.cpp + src/physics/ODEBox.cpp src/physics/WorldPhysics.cpp src/sensors/CameraSensor.cpp diff --git a/sim/src/core/NodeManager.cpp b/sim/src/core/NodeManager.cpp index 5f0c2a109..cad56445e 100644 --- a/sim/src/core/NodeManager.cpp +++ b/sim/src/core/NodeManager.cpp @@ -243,8 +243,10 @@ 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 = PhysicsMapper::newODEObject(control->sim->getPhysics(), nodeS); + std::cout << "DEBUGGG: create ODEObject pointer with getPhyiscs NodeManager " << __FILE__ << ":" << __LINE__ << std::endl; + + if (newNodeInterface.get() == nullptr) { // if no node was created in physics // delete the objects newNode.reset(); @@ -253,6 +255,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); diff --git a/sim/src/core/PhysicsMapper.cpp b/sim/src/core/PhysicsMapper.cpp index 3ce215069..1ecbf1964 100644 --- a/sim/src/core/PhysicsMapper.cpp +++ b/sim/src/core/PhysicsMapper.cpp @@ -38,13 +38,65 @@ namespace mars { return std::static_pointer_cast(worldPhysics); } - std::shared_ptr PhysicsMapper::newNodePhysics(std::shared_ptr worldPhysics) { + std::shared_ptr PhysicsMapper::newODEObject(std::shared_ptr worldPhysics, NodeData * nodeData) { // 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 nodeInterface; + // first we create a ode geometry for the node + switch(nodeData->physicMode) { +// case NODE_TYPE_MESH: +// ret = createMesh(node); +// break; + case NODE_TYPE_BOX: + nodeInterface = std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + 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 + std::cout << "DEBUGGG: default of switch case in PhysicsMapper " << __FILE__ << ":" << __LINE__ << std::endl; + return 0; + break; + } + + if( ! nodeInterface) { + // Error creating the physical Node + std::cout << "DEBUGGG: ODEObject creation failed in PhysicsMapper " << __FILE__ << ":" << __LINE__ << std::endl; + return 0; + } + + if ( nodeInterface ) + { + std::cout << "DEBUGGG: After switch: nodeInterface initialized and valid for some reason" << __FILE__ << ":" << __LINE__ << std::endl; + } + if ( nodeInterface.get() ) { + std::cout << "DEBUGGG: After switch: nodeInterface initialized and internal ptr valid for some reason" << __FILE__ << ":" << __LINE__ << std::endl; + } + if ( nodeInterface.get() == nullptr ) { + std::cout << "DEBUGGG: After switch: nodeInterface initialized and internal ptr == nullptr" << __FILE__ << ":" << __LINE__ << std::endl; + } + if ( nodeInterface == nullptr ) { + std::cout << "DEBUGGG: After switch: nodeInterface initialized and shared_ptr == nullptr" << __FILE__ << ":" << __LINE__ << std::endl; + } + + std::cout << "DEBUGGG: return NodeInterface as static cast of ODEObject" << __FILE__ << ":" << __LINE__ << std::endl; + return nodeInterface; + } - std::shared_ptr PhysicsMapper::newJointPhysics(std::shared_ptr worldPhysics) { + std::shared_ptr PhysicsMapper::newJointPhysics(std::shared_ptr worldPhysics) { std::shared_ptr jointPhysics = std::make_shared(worldPhysics); return std::static_pointer_cast(jointPhysics); } diff --git a/sim/src/core/PhysicsMapper.h b/sim/src/core/PhysicsMapper.h index 0ef303d34..9db16de02 100644 --- a/sim/src/core/PhysicsMapper.h +++ b/sim/src/core/PhysicsMapper.h @@ -34,7 +34,8 @@ #endif #include "WorldPhysics.h" -#include "NodePhysics.h" +#include "ODEBox.h" +#include "ODEObject.h" #include "JointPhysics.h" namespace mars { @@ -43,7 +44,9 @@ namespace mars { class PhysicsMapper { public: static std::shared_ptr newWorldPhysics(interfaces::ControlCenter *control); - static std::shared_ptr newNodePhysics(std::shared_ptr worldPhysics); + //TODO naming. Would prefer ODEObject here + // check other files if NodeInterface pointer would be preferable over ODEObject pointer? + static std::shared_ptr newODEObject(std::shared_ptr worldPhysics, interfaces::NodeData * nodeData); static std::shared_ptr newJointPhysics(std::shared_ptr worldPhysics); }; diff --git a/sim/src/physics/JointPhysics.cpp b/sim/src/physics/JointPhysics.cpp index 7222900c5..62a32ffca 100644 --- a/sim/src/physics/JointPhysics.cpp +++ b/sim/src/physics/JointPhysics.cpp @@ -31,7 +31,7 @@ #include #include "JointPhysics.h" -#include "NodePhysics.h" +#include "ODEObject.h" #include @@ -119,8 +119,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); diff --git a/sim/src/physics/ODEBox.cpp b/sim/src/physics/ODEBox.cpp new file mode 100644 index 000000000..48cc4aa96 --- /dev/null +++ b/sim/src/physics/ODEBox.cpp @@ -0,0 +1,460 @@ +/* + * 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 . + * + */ + +#include "ODEBox.h" + +namespace mars { + namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEBox::ODEBox(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODEBox Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); + } + + ODEBox::~ODEBox(void) { + std::cout << "DEBUGGG: in ODEBox Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + } + + /** + * \brief The method creates an ode node, which properties are given by + * the NodeData param node. + * + * 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 + * + * post: + * - a physical node should be created in the world + * - the node properties should be set + */ + bool ODEBox::createNode(NodeData* node) { +#ifdef _VERIFY_WORLD_ + std::cout << "DEBUGGG: in ODEBox create Node " << __FILE__ << ":" << __LINE__ << std::endl; + 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 = createBox(node); + if(ret == 0) { + // Error createing the physical Node + return 0; + } + + // 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) { + 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()); + //TODO is this irrelevant as we are in ODEBox now? or is this still possible due to wrong parameter definition/ class selection + if(node->physicMode == NODE_TYPE_TERRAIN) { + dGeomGetQuaternion(nGeom, t1); + dQMultiply0(t2, tmp, t1); + dGeomSetQuaternion(nGeom, t2); + } + else + dGeomSetQuaternion(nGeom, tmp); + } + node_data.id = node->index; + dGeomSetData(nGeom, &node_data); + locker.unlock(); + setContactParams(node->c_params); + return 1; + } + LOG_WARN("ODEBox: tried to create a Box without there being a world."); + return 0; + } + + /** + * \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 + */ + //TODO Where is this called and how should it be handled?! + // Use switch for now and handle it via Registry later + bool ODEBox::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; + success = createBox(node); + 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; + } + + + /** + * The method creates an ode box representation of the given node. + * + */ + bool ODEBox::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; + } + +// /** +// * \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 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; +// } + + } // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/ODEBox.h b/sim/src/physics/ODEBox.h new file mode 100644 index 000000000..9092b87e2 --- /dev/null +++ b/sim/src/physics/ODEBox.h @@ -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 . + * + */ + + /** + * \file ODEBox.h + * \author Malte Roemmermann, Leon Danter + * \brief "ODEBox" implements an ODE box geometry and collision using the ODEObject class + * + */ + +#ifndef ODE_BOX_H +#define ODE_BOX_H + +#ifdef _PRINT_HEADER_ + #warning "ODEBox.h" +#endif + +#include +#include "ODEObject.h" + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { + namespace sim { + + /** + * The class that implements the NodeInterface interface. + * + */ + class ODEBox : public ODEObject { + public: + ODEBox(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEBox(void); + + //TODO createGeometry vs createCollision? nBody vs nCollision + // review header comment on ODEBox + virtual bool createNode(interfaces::NodeData *node) override; + virtual bool changeNode(interfaces::NodeData *node) override; + bool createBox(interfaces::NodeData *node); + }; + + } // end of namespace sim +} // end of namespace mars + +#endif // ODE_BOX_H diff --git a/sim/src/physics/ODEObject.cpp b/sim/src/physics/ODEObject.cpp new file mode 100644 index 000000000..6c776b84c --- /dev/null +++ b/sim/src/physics/ODEObject.cpp @@ -0,0 +1,1384 @@ +/* + * 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 . + * + */ + +//TODO cleanup includes +#include "ODEObject.h" +#include "../sensors/RotatingRaySensor.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + + +namespace mars { + namespace sim { + + using namespace utils; + using namespace interfaces; + + /** + * \brief Creates a empty node objekt. + * + * \pre + * - the pointer to the physics Interface should be correct. + * This implementation can be a bad trap. The class that implements the + * physics interface, have to inherit from the interface at first, + * otherwise this implementation will cause bad error while pointing + * an incorrect adresses. + * + * \post + * - the class should have saved the pointer to the physics implementation + * - the body and geom should be initialized to 0 + */ + ODEObject::ODEObject(std::shared_ptr world, NodeData * nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODEObject Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + 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; + dMassSetZero(&nMass); + } + + /** + * \brief Destroys the node in the physical world. + * + * pre: + * - theWorld is the correct world object + * + * post: + * - all physical representation of the node should be cleared + * + * are the geom and the body realy all thing to take care of? + */ + ODEObject::~ODEObject(void) { + std::cout << "DEBUGGG: in ODEObject Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + std::vector::iterator iter; + MutexLocker locker(&(theWorld->iMutex)); + + if(nBody) theWorld->destroyBody(nBody, this); + + 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){ + delete ((*iter).gd); + (*iter).gd = 0; + } + dGeomDestroy((*iter).geom); + sensor_list.erase(iter); + } + if(myTriMeshData) dGeomTriMeshDataDestroy(myTriMeshData); + } + + dReal heightfield_callback(void* pUserData, int x, int z ) { + return ((ODEObject*)pUserData)->heightCallback(x, z); + } + + /** + * \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 ODEObject::getPosition(Vector* pos) const { + MutexLocker locker(&(theWorld->iMutex)); + if(nBody) { + const dReal* tmp = dGeomGetPosition(nGeom); + pos->x() = (sReal)tmp[0]; + pos->y() = (sReal)tmp[1]; + pos->z() = (sReal)tmp[2]; + /* + if(composite) { + tmp = dGeomGetOffsetPosition(nGeom); + pos->x += (sReal)tmp[0]; + pos->y += (sReal)tmp[1]; + pos->z += (sReal)tmp[2]; + }*/ + } + else if(nGeom) { + const dReal* tmp = dGeomGetPosition(nGeom); + pos->x() = (sReal)tmp[0]; + pos->y() = (sReal)tmp[1]; + pos->z() = (sReal)tmp[2]; + } + else { + pos->x() = 0; + pos->y() = 0; + pos->z() = 0; + } + } + + /** + * \brief The method sets the position of the physical node model to the + * position of the param. If move_group is set, all nodes of a composite + * group will be moved, otherwise only this node will be moved. A vector + * from the old position to the new will be returned. + * + * I don't know if we should use this function in a way like it is + * implemented now. The pre and post conditions could loke like this: + * + * pre: + * - there should be a physical representation of the node + * - the pos param should point to a correct position struct + * + * post: + * - if there is a physically representation and the node is movable + * set the position of the corresponding body to the given parameter + * - otherwise, we have to do nothing + */ + const Vector ODEObject::setPosition(const Vector &pos, bool move_group) { + const dReal *tpos; + const dReal *tpos2; + dReal npos[3]; + Vector offset; + MutexLocker locker(&(theWorld->iMutex)); + + if(composite) { + if(move_group) { + /* + brot = dBodyGetQuaternion(nBody); + tpos2 = dGeomGetOffsetPosition(nGeom); + tpos = dBodyGetPosition(nBody); + dQtoR(brot, R); + dMULTIPLY0_331(npos, R, tpos2); + offset.x = pos->x - (sReal)(tpos[0]+npos[0]); + offset.y = pos->y - (sReal)(tpos[1]+npos[1]); + offset.z = pos->z - (sReal)(tpos[2]+npos[2]); + */ + tpos2 = dGeomGetPosition(nGeom); + offset.x() = pos.x() - (sReal)(tpos2[0]); + offset.y() = pos.y() - (sReal)(tpos2[1]); + offset.z() = pos.z() - (sReal)(tpos2[2]); + tpos = dBodyGetPosition(nBody); + dBodySetPosition(nBody, tpos[0] + (dReal)offset.x(), + tpos[1] + (dReal)offset.y(), + tpos[2] + (dReal)offset.z()); + return offset; + } + else { + tpos = dBodyGetPosition(nBody); + tpos2 = dGeomGetOffsetPosition(nGeom); + npos[0] = tpos[0] + tpos2[0]; + npos[1] = tpos[1] + tpos2[1]; + npos[2] = tpos[2] + tpos2[2]; + offset.x() = pos.x() - (sReal)(npos[0]); + offset.y() = pos.y() - (sReal)(npos[1]); + offset.z() = pos.z() - (sReal)(npos[2]); + //std::cout << " " << pos.z(); + dGeomSetOffsetWorldPosition(nGeom, (dReal)pos.x(), (dReal)pos.y(), + (dReal)pos.z()); + // here we have to recalculate the mass + theWorld->resetCompositeMass(nBody); + return offset; + } + } + else { + if(nBody) { + tpos = dBodyGetPosition(nBody); + offset.x() = pos.x() - (sReal)(tpos[0]); + offset.y() = pos.y() - (sReal)(tpos[1]); + offset.z() = pos.z() - (sReal)(tpos[2]); + dBodySetPosition(nBody, (dReal)pos.x(), (dReal)pos.y(), (dReal)pos.z()); + return offset; + } + else if(nGeom) { + //tpos = dGeomGetPosition(nGeom); + //offset.x() = pos->x - (sReal)(tpos[0]); + //offset.y() = pos->y - (sReal)(tpos[1]); + //offset.z() = pos->z - (sReal)(tpos[2]); + dGeomSetPosition(nGeom, (dReal)pos.x(), (dReal)pos.y(), (dReal)pos.z()); + return offset; + } + } + return offset; + } + + /** + * \brief The method copies the Quaternion of the physically node at the + * adress of the Quaternion pointer q. + * + * pre: + * - there should be a physical representation of the node + * - the node should be movable + * - the q param should point to a correct Quaternion struct + * + * post: + * - if there is a physical representation and the node is movable + * the Quaternion struct should be filled with the physical rotation + * of the node + * - otherwise a standard return of zero rotation should be set + */ + void ODEObject::getRotation(Quaternion* q) const { + dQuaternion tmp; + MutexLocker locker(&(theWorld->iMutex)); + + if(nBody || nGeom) { + dGeomGetQuaternion(nGeom, tmp); + q->x() = (sReal)tmp[1]; + q->y() = (sReal)tmp[2]; + q->z() = (sReal)tmp[3]; + q->w() = (sReal)tmp[0]; + } + else { + q->x() = (sReal)0; + q->y() = (sReal)0; + q->z() = (sReal)0; + q->w() = (sReal)1; + } + } + + /** + * \brief The method copies the linear velocity of the physically node at the + * adress of the linear_vel pointer vel. + * + * pre: + * - there should be a physical representation of the node + * - the node should be movable + * - the vel param should point to a correct linear_vel struct + * + * post: + * - if there is a physical representation and the node is movable + * the linear_vel struct should be filled with the physical linar + * velocity of the node + * - otherwise a standard return of zero velocity should be set + */ + void ODEObject::getLinearVelocity(Vector* vel) const { + const dReal *tmp; + MutexLocker locker(&(theWorld->iMutex)); + + if(nBody) { + tmp = dBodyGetLinearVel(nBody); + vel->x() = (sReal)tmp[0]; + vel->y() = (sReal)tmp[1]; + vel->z() = (sReal)tmp[2]; + } + else { + vel->x() = (sReal)0; + vel->y() = (sReal)0; + vel->z() = (sReal)0; + } + } + + /** + * \brief The method copies the angular velocity of the physically node at the + * adress of the angular_vel pointer vel. + * + * pre: + * - there should be a physical representation of the node + * - the node should be movable + * - the vel param should point to a correct angular_vel struct + * + * post: + * - if there is a physical representation and the node is movable + * the angular_vel struct should be filled with the physical angular + * velocity of the node + * - otherwise a standard return of zero velocity should be set + */ + void ODEObject::getAngularVelocity(Vector* vel) const { + const dReal *tmp; + MutexLocker locker(&(theWorld->iMutex)); + + if(nBody) { + tmp = dBodyGetAngularVel(nBody); + vel->x() = (sReal)tmp[0]; + vel->y() = (sReal)tmp[1]; + vel->z() = (sReal)tmp[2]; + } + else { + vel->x() = (sReal)0; + vel->y() = (sReal)0; + vel->z() = (sReal)0; + } + } + + /** + * \brief The method copies the force of the physically node at the + * adress of the force pointer force. + * + * pre: + * - there should be a physical representation of the node + * - the node should be movable + * - the f param should point to a correct force struct + * + * post: + * - if there is a physical representation and the node is movable + * the force struct should be filled with the physical + * force of the node + * - otherwise a standard return of zero force should be set + */ + void ODEObject::getForce(Vector* f) const { + const dReal *tmp; + MutexLocker locker(&(theWorld->iMutex)); + + if(nBody) { + tmp = dBodyGetForce(nBody); + f->x() = (sReal)tmp[0]; + f->y() = (sReal)tmp[1]; + f->z() = (sReal)tmp[2]; + } + else { + f->x() = (sReal)0; + f->y() = (sReal)0; + f->z() = (sReal)0; + } + } + + /** + * \brief The method copies the torque of the physically node at the + * adress of the torque pointer force. + * + * pre: + * - there should be a physical representation of the node + * - the node should be movable + * - the t param should point to a correct torque struct + * + * post: + * - if there is a physical representation and the node is movable + * the torque struct should be filled with the physical torque + * of the node + * - otherwise a standard return of zero torque should be set + */ + void ODEObject::getTorque(Vector *t) const { + const dReal *tmp; + MutexLocker locker(&(theWorld->iMutex)); + + if(nBody) { + tmp = dBodyGetTorque(nBody); + t->x() = (sReal)tmp[0]; + t->y() = (sReal)tmp[1]; + t->z() = (sReal)tmp[2]; + } + else { + t->x() = (sReal)0; + t->y() = (sReal)0; + t->z() = (sReal)0; + } + } + + + /** + * \brief This method sets the rotation of the physically node. + * + * I don't if and how to use this function yet. ^-^ + * If we need it, the pre and post conditions are like them in the set + * position method. + */ + const Quaternion ODEObject::setRotation(const Quaternion &q, bool move_group) { + dQuaternion tmp, tmp2, tmp3, tmp4, tmp5; + const dReal *brot, *bpos, *gpos; + Quaternion q2; + dMatrix3 R; + dVector3 pos, new_pos, new2_pos; + MutexLocker locker(&(theWorld->iMutex)); + + pos[0] = pos[1] = pos[2] = 0; + tmp[1] = (dReal)q.x(); + tmp[2] = (dReal)q.y(); + tmp[3] = (dReal)q.z(); + tmp[0] = (dReal)q.w(); + if(nBody) { + bpos = dBodyGetPosition(nBody); + pos[0] = bpos[0]; + pos[1] = bpos[1]; + pos[2] = bpos[2]; + brot = dBodyGetQuaternion(nBody); + tmp4[0] = brot[0]; + tmp4[1] = brot[1]; + tmp4[2] = brot[2]; + tmp4[3] = brot[3]; + if(composite && !move_group) { + dGeomGetQuaternion(nGeom, tmp2); + dGeomSetOffsetWorldQuaternion(nGeom, tmp); + } + else if (composite) { + dGeomGetQuaternion(nGeom, tmp2); + dGeomGetOffsetQuaternion(nGeom, tmp3); + tmp3[1] *= -1; + tmp3[2] *= -1; + tmp3[3] *= -1; + dQMultiply0(tmp5, tmp, tmp3); + dBodySetQuaternion(nBody, tmp5); + } + else { + tmp2[0] = brot[0]; + tmp2[1] = brot[1]; + tmp2[2] = brot[2]; + tmp2[3] = brot[3]; + dBodySetQuaternion(nBody, tmp); + } + } + else if(nGeom) { + dGeomGetQuaternion(nGeom, tmp2); + dGeomSetQuaternion(nGeom, tmp); + } + dQMultiply2(tmp3, tmp, tmp2); + q2.x() = (sReal)tmp3[1]; + q2.y() = (sReal)tmp3[2]; + q2.z() = (sReal)tmp3[3]; + q2.w() = (sReal)tmp3[0]; + if(nBody && composite && move_group) { + gpos = dGeomGetOffsetPosition(nGeom); + dQtoR(tmp4, R); + dMULTIPLY0_331(new_pos, R, gpos); + pos[0] += new_pos[0]; + pos[1] += new_pos[1]; + pos[2] += new_pos[2]; + new_pos[0] *= -1; + new_pos[1] *= -1; + new_pos[2] *= -1; + dQtoR(tmp3, R); + dMULTIPLY0_331(new2_pos, R, new_pos); + new_pos[0] = pos[0]+new2_pos[0]; + new_pos[1] = pos[1]+new2_pos[1]; + new_pos[2] = pos[2]+new2_pos[2]; + dBodySetPosition(nBody, new_pos[0], new_pos[1], new_pos[2]); + } + return q2; + } + + /** + * \brief This function sets the pointer to the physical world object. + * + * I don't think that we need this function. + */ + void ODEObject::setWorldObject(std::shared_ptr world) { + theWorld = std::dynamic_pointer_cast(world); + } + + /** + *\brief return the body; + * this function is created to make it possible to get the + * body from joint physics + *TO DO : test if the Node has a body + */ + dBodyID ODEObject::getBody() const { + return nBody; + } + + /** + * This method sets some properties for the node. The properties includes + * the posistion, the rotation, the movability and the coposite group number + * + */ + void ODEObject::setProperties(NodeData* node) { + bool body_created = 1; + dQuaternion tmp; + + tmp[1] = (dReal)node->rot.x(); + tmp[2] = (dReal)node->rot.y(); + tmp[3] = (dReal)node->rot.z(); + tmp[0] = (dReal)node->rot.w(); + + // first we must find or create a physically body for the node + // and connect the geometry to the body + if(node->groupID) { + body_created = theWorld->getCompositeBody(node->groupID, &nBody, this); + composite = true; + } + else { + if(!nBody) nBody = dBodyCreate(theWorld->getWorld()); + } + //dBodySetLinearDamping(nBody, 0.5); + //dBodySetAngularDamping(nBody, 0.5); + dGeomSetBody(nGeom, nBody); + + // if a new body was created, we have to set the initial position + // and rotation of the body, otherwise have to set the position + // and rotation of the geom + if(body_created) { + dBodySetMass(nBody, &nMass); + dBodySetPosition(nBody, (dReal)(node->pos.x()), + (dReal)(node->pos.y()), + (dReal)(node->pos.z())); + // ## the rotation has to be defined as quanternion or matrix? ## + dBodySetQuaternion(nBody, tmp); + } + else if(node->density > 0 || node->mass > 0) { + dMass bodyMass; + + dBodyGetMass(nBody, &bodyMass); + dGeomSetOffsetWorldPosition(nGeom, (dReal)(node->pos.x()), + (dReal)(node->pos.y()), + (dReal)(node->pos.z())); + // ## the same for the rotation here ## + dGeomSetOffsetWorldQuaternion(nGeom, tmp); + addMassToCompositeBody(nBody, &bodyMass); + dBodySetMass(nBody, &bodyMass); + } +#ifdef _DEBUG_MASS_ + fprintf(stderr, "%mass id: %d %g\n", node->index, nMass.mass); + fprintf(stderr, "\t%g\t%g\t%g\n", nMass.I[0], nMass.I[1], nMass.I[2]); + fprintf(stderr, "\t%g\t%g\t%g\n", nMass.I[4], nMass.I[5], nMass.I[6]); + fprintf(stderr, "\t%g\t%g\t%g\n", nMass.I[8], nMass.I[9], nMass.I[10]); +#endif + } + + /** + * \brief executes an rotation at a given point and returns the + * new position of the node + * + * pre: + * + * post: + */ + const Vector ODEObject::rotateAtPoint(const Vector &rotation_point, + const Quaternion &rotation, + bool move_group) { + dQuaternion tmp, tmp2, tmp3; + const dReal *bpos, *gpos, *brot; + dVector3 pos, new_pos; + Vector npos; + dMatrix3 R; + MutexLocker locker(&(theWorld->iMutex)); + + tmp[1] = (dReal)rotation.x(); + tmp[2] = (dReal)rotation.y(); + tmp[3] = (dReal)rotation.z(); + tmp[0] = (dReal)rotation.w(); + if(composite) { + brot = dBodyGetQuaternion(nBody); + dQMultiply0(tmp2, tmp, brot); + if(move_group) { + // we have to rotate the body and return the new position of the geom + dBodySetQuaternion(nBody, tmp2); + dQtoR(tmp, R); + bpos = dBodyGetPosition(nBody); + pos[0] = bpos[0] - (dReal)rotation_point.x(); + pos[1] = bpos[1] - (dReal)rotation_point.y(); + pos[2] = bpos[2] - (dReal)rotation_point.z(); + dMULTIPLY0_331(new_pos, R, pos); + npos.x() = new_pos[0] + (dReal)rotation_point.x(); + npos.y() = new_pos[1] + (dReal)rotation_point.y(); + npos.z() = new_pos[2] + (dReal)rotation_point.z(); + dBodySetPosition(nBody, (dReal)npos.x(), (dReal)npos.y(), (dReal)npos.z()); + + gpos = dGeomGetOffsetPosition(nGeom); + npos.x() += (sReal)(gpos[0]); + npos.y() += (sReal)(gpos[1]); + npos.z() += (sReal)(gpos[2]); + return npos; + } + else { + dGeomGetQuaternion(nGeom, tmp3); + dQMultiply0(tmp2, tmp, tmp3); + dNormalize4(tmp2); + dGeomSetOffsetWorldQuaternion(nGeom, tmp2); + + gpos = dGeomGetPosition(nGeom); + pos[0] = gpos[0] - (dReal)rotation_point.x(); + pos[1] = gpos[1] - (dReal)rotation_point.y(); + pos[2] = gpos[2] - (dReal)rotation_point.z(); + dQtoR(tmp, R); + dMULTIPLY0_331(new_pos, R, pos); + pos[0] = new_pos[0] + (dReal)rotation_point.x(); + pos[1] = new_pos[1] + (dReal)rotation_point.y(); + pos[2] = new_pos[2] + (dReal)rotation_point.z(); + dGeomSetOffsetWorldPosition(nGeom, pos[0], pos[1], pos[2]); + npos.x() = (sReal)(pos[0]); + npos.y() = (sReal)(pos[1]); + npos.z() = (sReal)(pos[2]); + return npos; + } + } + // the last two cases do in principle the same + // we have to rotate the geom and calculate the translation + /* + else if(nBody) { + + }*/ + else if(nGeom) { + dGeomGetQuaternion(nGeom, tmp2); + dQMultiply0(tmp3, tmp, tmp2); + dGeomSetQuaternion(nGeom, tmp3); + dQtoR(tmp, R); + gpos = dGeomGetPosition(nGeom); + pos[0] = gpos[0] - (dReal)rotation_point.x(); + pos[1] = gpos[1] - (dReal)rotation_point.y(); + pos[2] = gpos[2] - (dReal)rotation_point.z(); + dMULTIPLY0_331(new_pos, R, pos); + npos.x() = new_pos[0] + (dReal)rotation_point.x(); + npos.y() = new_pos[1] + (dReal)rotation_point.y(); + npos.z() = new_pos[2] + (dReal)rotation_point.z(); + dGeomSetPosition(nGeom, (dReal)npos.x(), (dReal)npos.y(), (dReal)npos.z()); + return npos; + } + return npos; + } + + /** + * \brief returns the ode mass object + * + * + * pre: + * + * post: + */ + dMass ODEObject::getODEMass(void) const { + return nMass; + } + + /** + * \brief Sets the linear velocity of a node + * + * pre: + * - the node should have a body + * + * post: + * - the linear velocity of the body should be set + */ + void ODEObject::setLinearVelocity(const Vector &velocity) { + MutexLocker locker(&(theWorld->iMutex)); + if(nBody) dBodySetLinearVel(nBody, (dReal)velocity.x(), + (dReal)velocity.y(), (dReal)velocity.z()); + } + + /** + * \brief Sets the angular velocity of a node + * + * pre: + * - the node should have a body + * + * post: + * - the angular velocity of the body should be set + */ + void ODEObject::setAngularVelocity(const Vector &velocity) { + MutexLocker locker(&(theWorld->iMutex)); + if(nBody) dBodySetAngularVel(nBody, (dReal)velocity.x(), + (dReal)velocity.y(), (dReal)velocity.z()); + } + + /** + * \brief Sets the force of a node + * + * pre: + * - the node should have a body + * + * post: + * - the force of the body should be set + */ + void ODEObject::setForce(const Vector &f) { + MutexLocker locker(&(theWorld->iMutex)); + if(nBody) dBodySetForce(nBody, (dReal)f.x(), + (dReal)f.y(), (dReal)f.z()); + } + + /** + * \brief Sets the torque of a node + * + * pre: + * - the node should have a body + * + * post: + * - the torque of the body should be set + */ + void ODEObject::setTorque(const Vector &t) { + MutexLocker locker(&(theWorld->iMutex)); + if(nBody) dBodySetTorque(nBody, (dReal)t.x(), + (dReal)t.y(), (dReal)t.z()); + } + + /** + * \brief Adds a off-center force to a node + * + * pre: + * - the node should have a body + * + * post: + * - the force should be added to the body + */ + void ODEObject::addForce(const Vector &f, const Vector &p) { + MutexLocker locker(&(theWorld->iMutex)); + if(nBody) { + dBodyAddForceAtPos(nBody, + (dReal)f.x(), (dReal)f.y(), (dReal)f.z(), + (dReal)p.x(), (dReal)p.y(), (dReal)p.z()); + } + } + /** + * \brief Adds a force to a node + * + * pre: + * - the node should have a body + * + * post: + * - the force should be added to the body + */ + void ODEObject::addForce(const Vector &f) { + MutexLocker locker(&(theWorld->iMutex)); + if(nBody) { + dBodyAddForce(nBody, (dReal)f.x(), (dReal)f.y(), (dReal)f.z()); + } + } + + /** + * \brief Adds a torque to a node + * + * pre: + * - the node should have a body + * + * post: + * - the torque should be added to the body + */ + void ODEObject::addTorque(const Vector &t) { + MutexLocker locker(&(theWorld->iMutex)); + if(nBody) dBodyAddTorque(nBody, (dReal)t.x(), (dReal)t.y(), (dReal)t.z()); + } + + bool ODEObject::getGroundContact(void) const { + if(nGeom) { + return node_data.num_ground_collisions; + } + return false; + } + + void ODEObject::getContactPoints(std::vector *contact_points) const { + contact_points->clear(); + if(nGeom) { + std::vector::const_iterator iter; + + for(iter=node_data.contact_points.begin(); + iter!=node_data.contact_points.end(); ++iter) + + contact_points->push_back((*iter)); + } + } + + void ODEObject::getContactIDs(std::list *ids) const { + ids->clear(); + if(nGeom) { + *ids = node_data.contact_ids; + } + } + + + sReal ODEObject::getGroundContactForce(void) const { + std::vector::const_iterator iter; + dReal force[3] = {0,0,0}; + + if(nGeom) { + for(iter = node_data.ground_feedbacks.begin(); + iter != node_data.ground_feedbacks.end(); iter++) { + if(node_data.node1) { + force[0] += (*iter)->f1[0]; + force[1] += (*iter)->f1[1]; + force[2] += (*iter)->f1[2]; + } + else { + force[0] += (*iter)->f2[0]; + force[1] += (*iter)->f2[1]; + force[2] += (*iter)->f2[2]; + } + } + } + return dLENGTH(force); + } + + const Vector ODEObject::getContactForce(void) const { + std::vector::const_iterator iter; + dReal force[3] = {0,0,0}; + + if(nGeom) { + for(iter = node_data.ground_feedbacks.begin(); + iter != node_data.ground_feedbacks.end(); iter++) { + if(node_data.node1) { + force[0] += (*iter)->f1[0]; + force[1] += (*iter)->f1[1]; + force[2] += (*iter)->f1[2]; + } + else { + force[0] += (*iter)->f2[0]; + force[1] += (*iter)->f2[1]; + force[2] += (*iter)->f2[2]; + } + } + } + return Vector(force[0], force[1], force[2]); + } + + void ODEObject::addCompositeOffset(dReal x, dReal y, dReal z) { + // no lock because physics internal functions get locked elsewhere + const dReal *gpos; + + gpos = dGeomGetPosition(nGeom); + dGeomSetOffsetWorldPosition(nGeom, gpos[0]+x, gpos[1]+y, gpos[2]+z); + } + + void ODEObject::addMassToCompositeBody(dBodyID theBody, dMass *bodyMass) { + // no lock because physics internal functions get locked elsewhere + const dReal *rotation, *pos, *brot; + dReal tpos[3]; + dMatrix3 R; + dMass myMass = nMass; + + // first rotate and translate the mass and add it to the body + // we get the rotation and translation relative to the body-frame + // from the geom + rotation = dGeomGetOffsetRotation(nGeom); + pos = dGeomGetOffsetPosition(nGeom); + dMassRotate(&myMass, rotation); + dMassTranslate(&myMass, pos[0], pos[1], pos[2]); + dMassAdd(bodyMass, &myMass); + // the mass displacement is in bodyframe + // to get the displacement in world koordinates + // we have to rotate the vector + brot = dBodyGetQuaternion(theBody); + dRfromQ(R, brot); + dMULTIPLY0_331(tpos, R, bodyMass->c); + // then we can add the vector to the geoms and body + theWorld->moveCompositeMassCenter(theBody, tpos[0], tpos[1], tpos[2]); + dMassTranslate(bodyMass, -bodyMass->c[0], -bodyMass->c[1], -bodyMass->c[2]); + } + + 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); + + *tMass = nMass; + dMassRotate(tMass, rot); + dMassTranslate(tMass, pos[0], pos[1], pos[2]); + } + + dReal ODEObject::heightCallback(int x, int y) { + + return (dReal)height_data[(y*terrain->width)+x]*terrain->scale; + } + + void ODEObject::setContactParams(contact_params& c_params) { + MutexLocker locker(&(theWorld->iMutex)); + node_data.c_params = c_params; + if(nGeom) { + dGeomSetCollideBits(nGeom, c_params.coll_bitmask); + dGeomSetCategoryBits(nGeom, c_params.coll_bitmask); + } + } + + /** + * \brief This function adds a new sensor to the physical node. + * Sensors that are implemented are a "ray sensor" and a "multi ray sensor" + * + * + * pre: + * - the BaseSensor should contain a complete and correct configuration + * - the world have to exists because geoms have to be created + * - this Node should have an existing geom + * + * post: + * - every new sensor has to be added to a list + * - the memory for the sensor data has to be allocated + * - the physical elements for the sensor had to be created + */ + void ODEObject::addSensor(BaseSensor* sensor) { + MutexLocker locker(&(theWorld->iMutex)); + int i; + geom_data* gd; + sensor_list_element sle; + const dReal *pos = dGeomGetPosition(nGeom); + const dReal *rot = dGeomGetRotation(nGeom); + Vector direction; + dVector3 dest, tmp; + //sReal rad_angle, rad_steps, rad_start; + double rad_steps, rad_start; + + + BasePolarIntersectionSensor *polarSensor; + polarSensor = dynamic_cast(sensor); + + //case SENSOR_TYPE_RAY: + if(polarSensor){ + sle.sensor = sensor; + sle.updateTime = 0.0; + //sensor.count_data = sensor.resolution; + //sensor.data = (sReal*)malloc(sensor.resolution * sizeof(sReal)); + + mars::sim::RotatingRaySensor* rotRaySensor = dynamic_cast(sensor); + if(rotRaySensor){ + int N = rotRaySensor->getNumberRays(); + std::vector& directions = rotRaySensor->getDirections(); + assert(N == directions.size()); + + // Requests and adds the single rays using the local sensor frame. + for(i=0; isense_contact_force = 0; + (*polarSensor)[i] = gd->value = polarSensor->maxDistance;//sensor.max_distance; + gd->ray_sensor = 1; + gd->parent_geom = nGeom; + gd->parent_body = nBody; + sle.geom = dCreateRay(NULL, polarSensor->maxDistance); + dGeomRaySetClosestHit(sle.geom, 1); + dGeomSetCollideBits(sle.geom, 32768); + dGeomSetCategoryBits(sle.geom, 32768); + + // Use the precalculated ray directions of the sensor. + direction = /*polarSensor->getOrientation() **/ directions[i]; + sle.ray_direction = direction; + tmp[0] = direction.x(); + tmp[1] = direction.y(); + tmp[2] = direction.z(); + dMULTIPLY0_331(dest, rot, tmp); + dGeomRaySet(sle.geom, pos[0], pos[1], pos[2], dest[0], dest[1], dest[2]); + sle.gd = gd; + sle.index = i; + sensor_list.push_back(sle); + dGeomSetData(sle.geom, gd); + //dGeomRaySetParams(sle.geom, 1, 1); + dGeomSetCollideBits(sle.geom, COLLIDE_MASK_SENSOR); + dGeomSetCategoryBits(sle.geom, COLLIDE_MASK_SENSOR); + dGeomDisable(sle.geom); + } + } else { + //rad_angle = polarSensor->widthX*; //M_PI*sensor.flare_angle/180; + rad_steps = polarSensor->getCols(); //rad_angle/(sReal)(sensor.resolution-1); + rad_start = -((rad_steps-1)/2.0)*polarSensor->stepX; //Starting to Left, because 0 is in front and rock convention posive CCW //(M_PI-rad_angle)/2; + if(rad_steps == 1){ + rad_start = 0; + } + for(i=0; isense_contact_force = 0; + (*polarSensor)[i] = gd->value = polarSensor->maxDistance;//sensor.max_distance; + + gd->ray_sensor = 1; + gd->parent_geom = nGeom; + gd->parent_body = nBody; + sle.geom = dCreateRay(NULL, polarSensor->maxDistance); + dGeomRaySetClosestHit(sle.geom, 1); + dGeomSetCollideBits(sle.geom, 32768); + dGeomSetCategoryBits(sle.geom, 32768); + direction = Vector(cos(rad_start+i*polarSensor->stepX), + sin(rad_start+i*polarSensor->stepX), 0); + //direction = QVRotate(sensor.rotation, direction); + direction = (polarSensor->getOrientation() * direction); + sle.ray_direction = direction; + tmp[0] = direction.x(); + tmp[1] = direction.y(); + tmp[2] = direction.z(); + dMULTIPLY0_331(dest, rot, tmp); + dGeomRaySet(sle.geom, pos[0], pos[1], pos[2], dest[0], dest[1], dest[2]); + sle.gd = gd; + sle.index = i; + sensor_list.push_back(sle); + dGeomSetData(sle.geom, gd); + //dGeomRaySetParams(sle.geom, 1, 1); + dGeomSetCollideBits(sle.geom, COLLIDE_MASK_SENSOR); + dGeomSetCategoryBits(sle.geom, COLLIDE_MASK_SENSOR); + dGeomDisable(sle.geom); + } + } + } + + BaseGridIntersectionSensor *polarGridSensor; + polarGridSensor = dynamic_cast(sensor); + + if(polarGridSensor){ + sle.sensor = sensor; + sle.updateTime = 0.0; + int cols, rows; + dVector3 dir={0,0,0,0}, xStep={0,0,0,0}, + yStep={0,0,0,0}, xOffset={0,0,0,0}, yOffset={0,0,0,0}; + + cols = polarGridSensor->getCols(); + rows = polarGridSensor->getRows(); + + tmp[0] = 0; + tmp[1] = 0; + tmp[2] = -1; + dMULTIPLY0_331(dir, rot, tmp); + tmp[0] = polarGridSensor->stepX; + tmp[1] = 0; + tmp[2] = 0; + //dMULTIPLY0_331(xStep, rot, tmp); + tmp[0] = 0; + tmp[1] = polarGridSensor->stepY; + tmp[2] = 0; + //dMULTIPLY0_331(yStep, rot, tmp); + + + for(int x=0; xsense_contact_force = 0; + (*polarGridSensor)[y*cols+x] = gd->value = polarGridSensor->maxDistance; + + gd->ray_sensor = 1; + gd->parent_geom = nGeom; + sle.geom = dCreateRay(theWorld->getSpace(), + polarGridSensor->maxDistance); + dGeomRaySetClosestHit(sle.geom, 1); + dGeomSetCollideBits(sle.geom, 32768); + dGeomSetCategoryBits(sle.geom, 32768); + + direction = (polarGridSensor->getOrientation() * + Vector(0.0, 0.0, -1.0)); + sle.ray_direction = direction; + tmp[0] = direction.x(); + tmp[1] = direction.y(); + tmp[2] = direction.z(); + dMULTIPLY0_331(dest, rot, tmp); + xOffset[0] = -cols*0.5*xStep[0] + x*xStep[0]; + xOffset[1] = -cols*0.5*xStep[1] + x*xStep[1]; + xOffset[2] = -cols*0.5*xStep[2] + x*xStep[2]; + + yOffset[0] = -rows*0.5*yStep[0] + y*yStep[0]; + yOffset[1] = -rows*0.5*yStep[1] + y*yStep[1]; + yOffset[2] = -rows*0.5*yStep[2] + y*yStep[2]; + + sle.ray_pos_offset.x() = xOffset[0] + yOffset[0]; + sle.ray_pos_offset.y() = xOffset[1] + yOffset[1]; + sle.ray_pos_offset.z() = xOffset[2] + yOffset[2]; + + dGeomRaySet(sle.geom, pos[0] + xOffset[0] + yOffset[0], + pos[1] + xOffset[1] + yOffset[1], + pos[2] + xOffset[2] + yOffset[2], + dest[0], dest[1], dest[2]); + sle.gd = gd; + sle.index = y*cols+x; + sensor_list.push_back(sle); + dGeomSetData(sle.geom, gd); + //dGeomRaySetParams(sle.geom, 1, 1); + dGeomSetCollideBits(sle.geom, COLLIDE_MASK_SENSOR); + dGeomSetCategoryBits(sle.geom, COLLIDE_MASK_SENSOR); + dGeomDisable(sle.geom); + + } + } + } + } + + void ODEObject::removeSensor(BaseSensor *sensor) { + MutexLocker locker(&(theWorld->iMutex)); + std::vector::iterator iter; + for (iter = sensor_list.begin(); iter != sensor_list.end(); ) { + if (iter->sensor == sensor) { + free(iter->gd); + dGeomDestroy(iter->geom); + iter = sensor_list.erase(iter); + } else + ++iter; + } + } + + /** + * \brief This function copies all sensor values to the specific allocated + * memory + * + * pre: + * + * post: + */ + void ODEObject::handleSensorData(bool physics_thread) { + if(!physics_thread) return; + MutexLocker locker(&(theWorld->iMutex)); + std::vector::iterator iter; + const dReal* pos = dGeomGetPosition(nGeom); + const dReal* rot = dGeomGetRotation(nGeom); + dVector3 dest, tmp, posOffset; + dReal steps_size = 1.0, length = 0.0; + bool done = false; + int steps = 0; + dReal worldStep = theWorld->getWorldStep(); + // RotatingRaySensor + utils::Vector tmpV; + utils::Quaternion turnrotation; + turnrotation.setIdentity(); + std::set ids_rotating_ray_sensors; + + //New Code + int i=0; + for(iter = sensor_list.begin(); iter != sensor_list.end(); iter++) { + i+=1; + if((double)iter->sensor->updateRate * 0.001 > worldStep) { + iter->updateTime += worldStep; + if(iter->updateTime < 0.001*iter->sensor->updateRate) continue; + iter->updateTime -= 0.001*iter->sensor->updateRate; + } + BasePolarIntersectionSensor *polarSensor = dynamic_cast((*iter).sensor); + if(polarSensor){ + sensor_list_element elem = *iter; + + tmpV[0] = elem.ray_direction.x(); + tmpV[1] = elem.ray_direction.y(); + tmpV[2] = elem.ray_direction.z(); + + // Applies orientation_offset (z-Rotation) to the laser rays. + mars::sim::RotatingRaySensor *rotRaySensor = dynamic_cast((*iter).sensor); + if(rotRaySensor){ + std::set::iterator it = ids_rotating_ray_sensors.find(rotRaySensor->id); + // Takes care that each rotating ray sensor is only turned once (sensor_list contains each ray independently). + if(it == ids_rotating_ray_sensors.end()) { + //turnrotation.setIdentity(); // If we set it to identity first, receiveSensorData will not be called anymore.. wtf!? + turnrotation = rotRaySensor->turn(); + ids_rotating_ray_sensors.insert(rotRaySensor->id); + } + //fprintf(stderr, "tmp[%i]: %f, %f, %f\n", i, tmpV.x(), tmpV.y(), tmpV.z()); + tmpV = turnrotation * tmpV; + //fprintf(stderr, "tmp[%i]: %f, %f, %f\n", i, tmpV.x(), tmpV.y(), tmpV.z()); + } + tmp[0] = tmpV.x();//elem.ray_direction.x(); + tmp[1] = tmpV.y();//elem.ray_direction.y(); + tmp[2] = tmpV.z();//elem.ray_direction.z(); + dMULTIPLY0_331(dest, rot, tmp); + + if(physics_thread) { + steps = 0; + length = 0.0; + done = false; + dGeomEnable(elem.geom); + // make here the collision check + while (!done) { + dGeomRaySet(elem.geom, + pos[0] + dest[0]*steps_size*steps, + pos[1] + dest[1]*steps_size*steps, + pos[2] + dest[2]*steps_size*steps, + dest[0], dest[1], dest[2]); + if(length + steps_size < polarSensor->maxDistance) { + steps++; + dGeomRaySetLength(elem.geom, steps_size); + } + else { + dGeomRaySetLength(elem.geom, polarSensor->maxDistance- length); + done = true; + } + if(theWorld->handleCollision(elem.geom)) { + elem.gd->value += length; + done = true; + } + if(!done) length = steps_size*steps; + } + dGeomDisable(elem.geom); + } + (*polarSensor)[elem.index] = elem.gd->value; + elem.gd->value = polarSensor->maxDistance; + } + + BaseGridIntersectionSensor *polarGridSensor; + polarGridSensor = dynamic_cast(iter->sensor); + + if(polarGridSensor) { + sensor_list_element elem = *iter; + + if(physics_thread) { + + tmp[0] = elem.ray_direction.x(); + tmp[1] = elem.ray_direction.y(); + tmp[2] = elem.ray_direction.z(); + dMULTIPLY0_331(dest, rot, tmp); + + tmp[0] = elem.ray_pos_offset.x(); + tmp[1] = elem.ray_pos_offset.y(); + tmp[2] = elem.ray_pos_offset.z(); + dMULTIPLY0_331(posOffset, rot, tmp); + + dGeomEnable(elem.geom); + + dGeomRaySet(elem.geom, pos[0] + posOffset[0], + pos[1] + posOffset[1], + pos[2] + posOffset[2], + dest[0], dest[1], dest[2]); + + dGeomRaySetLength(elem.geom, polarGridSensor->maxDistance); + theWorld->handleCollision(elem.geom); + dGeomDisable(elem.geom); + } + (*polarGridSensor)[elem.index] = elem.gd->value; + elem.gd->value = polarGridSensor->maxDistance; + } + } // end for loop. + } + + /** + * \brief destroyes a node from the physics + * + * pre: + * + * post: + */ + 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; + } + + void ODEObject::setInertiaMass(NodeData* node) { + + dMassSetZero(&nMass); + nMass.mass = (dReal)node->mass; + nMass.I[0] = (dReal)node->inertia[0][0]; + nMass.I[1] = (dReal)node->inertia[0][1]; + nMass.I[2] = (dReal)node->inertia[0][2]; + nMass.I[3] = 0.0; + + nMass.I[4] = (dReal)node->inertia[1][0]; + nMass.I[5] = (dReal)node->inertia[1][1]; + nMass.I[6] = (dReal)node->inertia[1][2]; + nMass.I[7] = 0.0; + + nMass.I[8] = (dReal)node->inertia[2][0]; + nMass.I[9] = (dReal)node->inertia[2][1]; + nMass.I[10] = (dReal)node->inertia[2][2]; + nMass.I[11] = 0.0; + } + + void ODEObject::getMass(sReal *mass, sReal *inertia) const { + if(mass) *mass = nMass.mass; + if(inertia) { + inertia[0] = nMass.I[0]; + inertia[1] = nMass.I[1]; + inertia[2] = nMass.I[2]; + inertia[3] = nMass.I[4]; + inertia[4] = nMass.I[5]; + inertia[5] = nMass.I[6]; + inertia[6] = nMass.I[8]; + inertia[7] = nMass.I[9]; + inertia[8] = nMass.I[10]; + } + } + + std::vector ODEObject::addContacts(ContactsPhysics contacts, dWorldID world, dJointGroupID contactgroup){ + dVector3 v; + //dMatrix3 R; + dReal dot; + std::vector fbs; + std::shared_ptr> contactsPtr = contacts.contactsPtr; + const smurf::ContactParams contactParams = contacts.collidable->getContactParams(); + std::string nodeName = contacts.collidable->getName(); + for(int i=0; ioperator[](i).geom.normal[0]; + v[1] = contactsPtr->operator[](i).geom.normal[1]; + v[2] = contactsPtr->operator[](i).geom.normal[2]; + dot = dDOT(v, contactsPtr->operator[](i).fdir1); + dOPEC(v, *=, dot); + contactsPtr->operator[](i).fdir1[0] -= v[0]; + contactsPtr->operator[](i).fdir1[1] -= v[1]; + contactsPtr->operator[](i).fdir1[2] -= v[2]; + //dNormalize3(contactsPtr->operator[](0).fdir1); // Why 0 and not i? + dNormalize3(contactsPtr->operator[](i).fdir1); + //} + contactsPtr->operator[](i).geom.depth += (contactParams.depth_correction); + /* + // TODO: If after further testing this is not needed, then remove. + // Otherwise use a parameter for this + if(contactsPtr->operator[](i).geom.depth > 0.001) + { + 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; + } + */ + if(contactsPtr->operator[](i).geom.depth < 0.0) contactsPtr->operator[](i).geom.depth = 0.0; + //contactsPtr->operator[](0).geom.depth += (contactParams.depth_correction); // Why 0 and not i? + contactsPtr->operator[](i).geom.depth += (contactParams.depth_correction); + //if(contactsPtr->operator[](0).geom.depth < 0.0) contactsPtr->operator[](0).geom.depth = 0.0; // Why 0 and not i ? + dJointID c=dJointCreateContact(world, contactgroup, &contactsPtr->operator[](i)); + dJointFeedback *fb; + fb = (dJointFeedback*)malloc(sizeof(dJointFeedback)); + dJointSetFeedback(c, fb); // ODE + #ifdef DEBUG_ADD_CONTACTS + if (isnan(abs(fb->f1[0]))||isnan(abs(fb->f1[1]))||isnan(abs(fb->f1[2])) || isnan(abs(fb->f1[3])) || + isnan(abs(fb->t1[0]))||isnan(abs(fb->t1[1]))||isnan(abs(fb->t1[2])) || isnan(abs(fb->t1[3])) || + isnan(abs(fb->f2[0]))||isnan(abs(fb->f2[1]))||isnan(abs(fb->f2[2])) || isnan(abs(fb->f2[3])) || + isnan(abs(fb->t2[0]))||isnan(abs(fb->t2[1]))||isnan(abs(fb->t2[2])) || isnan(abs(fb->t2[3])) ) + { + LOG_WARN("Catched a nan in the feedback forces, won't be used"); + } + 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); + } // for numContacts + return fbs; + } + + void ODEObject::addContact(dJointID contactJointId, dContact contact, dJointFeedback* fb){ + Vector contact_point; + dJointAttach(contactJointId, nBody, 0); + node_data.num_ground_collisions ++; + contact_point.x() = contact.geom.pos[0]; + contact_point.y() = contact.geom.pos[1]; + contact_point.z() = contact.geom.pos[2]; + node_data.contact_ids.push_back(0); + node_data.contact_points.push_back(contact_point); + node_data.ground_feedbacks.push_back(fb); + node_data.node1 = true; + } + + + sReal ODEObject::getCollisionDepth(void) const { + if(nGeom && theWorld) { + return theWorld->getCollisionDepth(nGeom); + } + return 0.0; + } + + } // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/ODEObject.h b/sim/src/physics/ODEObject.h new file mode 100644 index 000000000..bd6d316e5 --- /dev/null +++ b/sim/src/physics/ODEObject.h @@ -0,0 +1,175 @@ +/* + * 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 ODEObject.h + * \author Malte Roemmermann, Leon Danter + * \brief "ODEObject" implements a factory to create physical ode objects for the nodes. + * + */ + +#ifndef ODE_OBJECT_H +#define ODE_OBJECT_H + +#ifdef _PRINT_HEADER_ + #warning "ODEObject.h" +#endif + +#include "WorldPhysics.h" + +#include + +#ifndef ODE11 + #define dTriIndex int +#endif + +//TODO move struct descriptions to seperate file! +namespace mars { + namespace sim { + + /* + * we need a data structure to handle different collision parameter + * and we need to save the collision_data somewhere + * in this first version, it is a hack to get the ground collisions + */ + struct geom_data { + void setZero(){ + num_ground_collisions = 0; + ray_sensor = 0; + sense_contact_force = 1; + value = 0; + c_params.setZero(); + } + + geom_data(){ + setZero(); + } + unsigned long id; + int num_ground_collisions; + std::vector contact_points; + std::list contact_ids; + std::vector ground_feedbacks; + bool node1; + interfaces::contact_params c_params; + bool ray_sensor; + bool sense_contact_force; + interfaces::sReal value; + dGeomID parent_geom; + dBodyID parent_body; + }; + + struct sensor_list_element { + interfaces::BaseSensor *sensor; + geom_data *gd; + dGeomID geom; + utils::Vector ray_direction; + utils::Vector ray_pos_offset; + unsigned int index; + dReal updateTime; + }; + + /** + * The class that implements the NodeInterface interface. + * + */ + class ODEObject : public interfaces::NodeInterface { + public: + ODEObject(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEObject(void); + + virtual bool createNode(interfaces::NodeData *node) override { + //TODO still need switch interface here? + std::cout << "DEBUGGG: createNode in ODEObject " << __FILE__ << ":" << __LINE__ << std::endl; + return true; + }; + virtual bool changeNode(interfaces::NodeData *node) override { + std::cout << "DEBUGGG: changeNode in ODEObject " << __FILE__ << ":" << __LINE__ << std::endl; + return true; + }; + 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; + virtual const utils::Quaternion setRotation(const utils::Quaternion &q, bool move_group); + virtual void setWorldObject(std::shared_ptr world); + virtual void getLinearVelocity(utils::Vector *vel) const; + virtual void getAngularVelocity(utils::Vector *vel) const; + virtual void getForce(utils::Vector *f) const; + virtual void getTorque(utils::Vector *t) const; + virtual const utils::Vector rotateAtPoint(const utils::Vector &rotation_point, + const utils::Quaternion &rotation, + bool move_group); + virtual void setLinearVelocity(const utils::Vector &velocity); + virtual void setAngularVelocity(const utils::Vector &velocity); + virtual void setForce(const utils::Vector &f); + virtual void setTorque(const utils::Vector &t); + virtual void addForce(const utils::Vector &f, const utils::Vector &p); + virtual void addForce(const utils::Vector &f); + virtual void addTorque(const utils::Vector &t); + virtual bool getGroundContact(void) const; + virtual void getContactPoints(std::vector *contact_points) const; + virtual void getContactIDs(std::list *ids) const; + virtual interfaces::sReal getGroundContactForce(void) const; + virtual void setContactParams(interfaces::contact_params &c_params); + virtual void addSensor(interfaces::BaseSensor *sensor); + virtual void removeSensor(interfaces::BaseSensor *sensor); + virtual void handleSensorData(bool physics_thread = true); + virtual void destroyNode(void); + virtual void getMass(interfaces::sReal *mass, interfaces::sReal *inertia=0) const; + virtual const utils::Vector getContactForce(void) const; + virtual void addContact(dJointID contactJointId, dContact contact, dJointFeedback* fb); + virtual std::vector addContacts(ContactsPhysics contacts, dWorldID world, dJointGroupID contactgroup); + virtual interfaces::sReal getCollisionDepth(void) const; + void addCompositeOffset(dReal x, dReal y, dReal z); + ///return the body; this function is created to make it possible to get the + ///body from joint physics s + dBodyID getBody() const; + dMass getODEMass(void) const; + void addMassToCompositeBody(dBodyID theBody, dMass *bodyMass); + void getAbsMass(dMass *pMass) const; + dReal heightCallback(int x, int y); + + 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); + }; + + } // end of namespace sim +} // end of namespace mars + +#endif // NODE_PHYSICS_H diff --git a/sim/src/physics/WorldPhysics.cpp b/sim/src/physics/WorldPhysics.cpp index 776a7ac07..c05a22c7c 100644 --- a/sim/src/physics/WorldPhysics.cpp +++ b/sim/src/physics/WorldPhysics.cpp @@ -37,7 +37,7 @@ */ #include "WorldPhysics.h" -#include "NodePhysics.h" +#include "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) { @@ -951,7 +951,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; @@ -971,7 +971,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 @@ -998,7 +998,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/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/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; From e3114b2a968b3edb5f147209a77159a1ef81f172 Mon Sep 17 00:00:00 2001 From: mlodhi Date: Thu, 11 Aug 2022 14:58:20 +0200 Subject: [PATCH 03/21] Use ODEObjectFactory --- sim/CMakeLists.txt | 3 ++ sim/src/core/NodeManager.cpp | 11 +++++++- sim/src/core/NodeManager.h | 9 ++++-- sim/src/core/PhysicsMapper.cpp | 50 ---------------------------------- 4 files changed, 20 insertions(+), 53 deletions(-) diff --git a/sim/CMakeLists.txt b/sim/CMakeLists.txt index 500ace94c..9289993ec 100644 --- a/sim/CMakeLists.txt +++ b/sim/CMakeLists.txt @@ -92,6 +92,8 @@ set(SOURCES_H # src/physics/NodePhysics.h src/physics/WorldPhysics.h src/physics/ContactsPhysics.hpp + src/physics/ODEObjectCreator.h + src/physics/ODEObjectFactory.h src/sensors/CameraSensor.h src/sensors/IDListConfig.h @@ -145,6 +147,7 @@ set(TARGET_SRC src/physics/ODEObject.cpp src/physics/ODEBox.cpp src/physics/WorldPhysics.cpp + src/physics/ODEObjectCreator.cpp src/sensors/CameraSensor.cpp src/sensors/Joint6DOFSensor.cpp diff --git a/sim/src/core/NodeManager.cpp b/sim/src/core/NodeManager.cpp index cad56445e..cb2368257 100644 --- a/sim/src/core/NodeManager.cpp +++ b/sim/src/core/NodeManager.cpp @@ -75,6 +75,13 @@ namespace mars { GraphicsUpdateInterface *gui = static_cast(this); control->graphics->addGraphicsUpdateInterface(gui); } + std::cout << "Here " << std::endl; + odeObjectFactory = new ODEObjectCreator(); + } + + NodeManager::~NodeManager() + { + delete odeObjectFactory; } @@ -243,7 +250,9 @@ namespace mars { // create the physical node data if(! (nodeS->noPhysical)){ // create an interface object to the physics - std::shared_ptr newNodeInterface = PhysicsMapper::newODEObject(control->sim->getPhysics(), nodeS); + //std::shared_ptr newNodeInterface = PhysicsMapper::newODEObject(control->sim->getPhysics(), nodeS); + std::shared_ptr newNodeInterface = odeObjectFactory->createObject(control->sim->getPhysics(), nodeS); + std::cout << "DEBUGGG: create ODEObject pointer with getPhyiscs NodeManager " << __FILE__ << ":" << __LINE__ << std::endl; if (newNodeInterface.get() == nullptr) { diff --git a/sim/src/core/NodeManager.h b/sim/src/core/NodeManager.h index 274916713..1a63ae09e 100644 --- a/sim/src/core/NodeManager.h +++ b/sim/src/core/NodeManager.h @@ -37,6 +37,9 @@ #include #include +#include +#include + namespace mars { namespace sim { @@ -54,8 +57,7 @@ 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, bool movable=false, @@ -163,6 +165,9 @@ namespace mars { const std::string &value); private: + + ODEObjectFactory* odeObjectFactory; + 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 1ecbf1964..c0579e9fd 100644 --- a/sim/src/core/PhysicsMapper.cpp +++ b/sim/src/core/PhysicsMapper.cpp @@ -42,56 +42,6 @@ namespace mars { // Create a nodePhysics with the worldPhysics as constructor parameter, then downcast to a Node interface and return std::shared_ptr nodeInterface; // first we create a ode geometry for the node - switch(nodeData->physicMode) { -// case NODE_TYPE_MESH: -// ret = createMesh(node); -// break; - case NODE_TYPE_BOX: - nodeInterface = std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); - 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 - std::cout << "DEBUGGG: default of switch case in PhysicsMapper " << __FILE__ << ":" << __LINE__ << std::endl; - return 0; - break; - } - - if( ! nodeInterface) { - // Error creating the physical Node - std::cout << "DEBUGGG: ODEObject creation failed in PhysicsMapper " << __FILE__ << ":" << __LINE__ << std::endl; - return 0; - } - - if ( nodeInterface ) - { - std::cout << "DEBUGGG: After switch: nodeInterface initialized and valid for some reason" << __FILE__ << ":" << __LINE__ << std::endl; - } - if ( nodeInterface.get() ) { - std::cout << "DEBUGGG: After switch: nodeInterface initialized and internal ptr valid for some reason" << __FILE__ << ":" << __LINE__ << std::endl; - } - if ( nodeInterface.get() == nullptr ) { - std::cout << "DEBUGGG: After switch: nodeInterface initialized and internal ptr == nullptr" << __FILE__ << ":" << __LINE__ << std::endl; - } - if ( nodeInterface == nullptr ) { - std::cout << "DEBUGGG: After switch: nodeInterface initialized and shared_ptr == nullptr" << __FILE__ << ":" << __LINE__ << std::endl; - } - - std::cout << "DEBUGGG: return NodeInterface as static cast of ODEObject" << __FILE__ << ":" << __LINE__ << std::endl; return nodeInterface; } From 9e3ff41a5c950912e0f510cc808a313d26b7ce91 Mon Sep 17 00:00:00 2001 From: mlodhi Date: Thu, 11 Aug 2022 17:09:24 +0200 Subject: [PATCH 04/21] Add ODEObjectFactory files --- sim/CMakeLists.txt | 2 + sim/src/core/NodeManager.cpp | 5 +- sim/src/core/Simulator.cpp | 8 +- sim/src/physics/NodePhysics.cpp | 16 ++++ sim/src/physics/NodePhysics.h | 5 ++ sim/src/physics/ODEBox.cpp | 5 +- sim/src/physics/ODEBox.h | 2 +- sim/src/physics/ODEMesh.cpp | 119 +++++++++++++++++++++++++++ sim/src/physics/ODEMesh.h | 60 ++++++++++++++ sim/src/physics/ODEObject.h | 19 +---- sim/src/physics/ODEObjectCreator.cpp | 43 ++++++++++ sim/src/physics/ODEObjectCreator.h | 24 ++++++ sim/src/physics/ODEObjectFactory.h | 15 ++++ sim/src/physics/ODESphere.cpp | 69 ++++++++++++++++ sim/src/physics/ODESphere.h | 60 ++++++++++++++ 15 files changed, 427 insertions(+), 25 deletions(-) create mode 100644 sim/src/physics/ODEMesh.cpp create mode 100644 sim/src/physics/ODEMesh.h create mode 100644 sim/src/physics/ODEObjectCreator.cpp create mode 100644 sim/src/physics/ODEObjectCreator.h create mode 100644 sim/src/physics/ODEObjectFactory.h create mode 100644 sim/src/physics/ODESphere.cpp create mode 100644 sim/src/physics/ODESphere.h diff --git a/sim/CMakeLists.txt b/sim/CMakeLists.txt index 9289993ec..25b5328ae 100644 --- a/sim/CMakeLists.txt +++ b/sim/CMakeLists.txt @@ -89,6 +89,7 @@ set(SOURCES_H src/physics/JointPhysics.h src/physics/ODEObject.h src/physics/ODEBox.h + src/physics/ODEMesh.h # src/physics/NodePhysics.h src/physics/WorldPhysics.h src/physics/ContactsPhysics.hpp @@ -146,6 +147,7 @@ set(TARGET_SRC # src/physics/NodePhysics.cpp src/physics/ODEObject.cpp src/physics/ODEBox.cpp + src/physics/ODEMesh.cpp src/physics/WorldPhysics.cpp src/physics/ODEObjectCreator.cpp diff --git a/sim/src/core/NodeManager.cpp b/sim/src/core/NodeManager.cpp index cb2368257..e4d516b83 100644 --- a/sim/src/core/NodeManager.cpp +++ b/sim/src/core/NodeManager.cpp @@ -75,12 +75,13 @@ namespace mars { GraphicsUpdateInterface *gui = static_cast(this); control->graphics->addGraphicsUpdateInterface(gui); } - std::cout << "Here " << std::endl; - odeObjectFactory = new ODEObjectCreator(); + std::cout << "Here 1" << std::endl; + odeObjectFactory = ODEObjectCreator::getInstance(); } NodeManager::~NodeManager() { + std::cout << "Destroyed " << std::endl; delete odeObjectFactory; } diff --git a/sim/src/core/Simulator.cpp b/sim/src/core/Simulator.cpp index f73981f3b..d12575560 100644 --- a/sim/src/core/Simulator.cpp +++ b/sim/src/core/Simulator.cpp @@ -288,10 +288,10 @@ 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->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"); diff --git a/sim/src/physics/NodePhysics.cpp b/sim/src/physics/NodePhysics.cpp index b3e7fe9ae..02a7c988c 100644 --- a/sim/src/physics/NodePhysics.cpp +++ b/sim/src/physics/NodePhysics.cpp @@ -36,6 +36,8 @@ */ #include "NodePhysics.h" +#include "ODEObjectFactory.h" +#include "ODEBoxCreater.h" #include "../sensors/RotatingRaySensor.h" #include @@ -147,6 +149,7 @@ namespace mars { node->mass, node->density); #endif MutexLocker locker(&(theWorld->iMutex)); + if(theWorld && theWorld->existsWorld()) { bool ret; //LOG_DEBUG("physicMode %d", node->physicMode); @@ -197,12 +200,25 @@ namespace mars { if(node->physicMode == NODE_TYPE_TERRAIN) { dGeomGetQuaternion(nGeom, t1); dQMultiply0(t2, tmp, t1); + dNormalize4(t2); dGeomSetQuaternion(nGeom, t2); } else dGeomSetQuaternion(nGeom, tmp); } node_data.id = node->index; + if(node->map.hasKey("c_filter_depth")) { + node_data.filter_depth = node->map["c_filter_depth"]; + } + if(node->map.hasKey("c_filter_angle")) { + node_data.filter_angle = node->map["c_filter_angle"]; + } + if(node->map.hasKey("c_filter_sphere")) { + node_data.filter_sphere.x() = node->map["c_filter_sphere"][0]; + node_data.filter_sphere.y() = node->map["c_filter_sphere"][1]; + node_data.filter_sphere.z() = 0.0;//node->map["c_filter_sphere"][2]; + node_data.filter_radius = node->map["c_filter_sphere"][3]; + } dGeomSetData(nGeom, &node_data); locker.unlock(); setContactParams(node->c_params); diff --git a/sim/src/physics/NodePhysics.h b/sim/src/physics/NodePhysics.h index 74aca058e..e876bc162 100644 --- a/sim/src/physics/NodePhysics.h +++ b/sim/src/physics/NodePhysics.h @@ -55,6 +55,9 @@ namespace mars { sense_contact_force = 1; value = 0; c_params.setZero(); + filter_depth = -1.; + filter_angle = -1.; + filter_radius = -1.0; } geom_data(){ @@ -72,6 +75,8 @@ namespace mars { interfaces::sReal value; dGeomID parent_geom; dBodyID parent_body; + dReal filter_depth, filter_angle, filter_radius; + utils::Vector filter_sphere; }; struct sensor_list_element { diff --git a/sim/src/physics/ODEBox.cpp b/sim/src/physics/ODEBox.cpp index 48cc4aa96..48276100f 100644 --- a/sim/src/physics/ODEBox.cpp +++ b/sim/src/physics/ODEBox.cpp @@ -29,7 +29,7 @@ namespace mars { ODEBox::ODEBox(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { // At this moment we have not much things to do here. ^_^ std::cout << "DEBUGGG: in ODEBox Constructor " << __FILE__ << ":" << __LINE__ << std::endl; - createNode(nodeData); + createNode(nodeData); // pass a function pointer? } ODEBox::~ODEBox(void) { @@ -49,6 +49,7 @@ namespace mars { * - a physical node should be created in the world * - the node properties should be set */ + bool ODEBox::createNode(NodeData* node) { #ifdef _VERIFY_WORLD_ std::cout << "DEBUGGG: in ODEBox create Node " << __FILE__ << ":" << __LINE__ << std::endl; @@ -61,7 +62,7 @@ namespace mars { MutexLocker locker(&(theWorld->iMutex)); if(theWorld && theWorld->existsWorld()) { bool ret; - ret = createBox(node); + ret = createBox(node); //could this be a lamda or a function pointer? if(ret == 0) { // Error createing the physical Node return 0; diff --git a/sim/src/physics/ODEBox.h b/sim/src/physics/ODEBox.h index 9092b87e2..fd65dac17 100644 --- a/sim/src/physics/ODEBox.h +++ b/sim/src/physics/ODEBox.h @@ -54,7 +54,7 @@ namespace mars { //TODO createGeometry vs createCollision? nBody vs nCollision // review header comment on ODEBox - virtual bool createNode(interfaces::NodeData *node) override; + virtual bool createNode(interfaces::NodeData *node) override; //this can go to ODEObject virtual bool changeNode(interfaces::NodeData *node) override; bool createBox(interfaces::NodeData *node); }; diff --git a/sim/src/physics/ODEMesh.cpp b/sim/src/physics/ODEMesh.cpp new file mode 100644 index 000000000..541fc41d8 --- /dev/null +++ b/sim/src/physics/ODEMesh.cpp @@ -0,0 +1,119 @@ +/* + * 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 . + * + */ + +#include "ODEMesh.h" + +namespace mars { + namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEMesh::ODEMesh(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODEMesh Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); + } + + ODEMesh::~ODEMesh(void) { + std::cout << "DEBUGGG: in ODEMesh Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + } + + /** + * The method creates an ode box representation of the given node. + * + */ + bool ODEMesh::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; + } + } // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/ODEMesh.h b/sim/src/physics/ODEMesh.h new file mode 100644 index 000000000..838dc8ed8 --- /dev/null +++ b/sim/src/physics/ODEMesh.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 ODEMesh.h + * \author Malte Roemmermann, Leon Danter + * \brief "ODEMesh" implements an ode mesh representation of the given node. + * + */ + +#ifndef ODE_MESH_H +#define ODE_MESH_H + +#ifdef _PRINT_HEADER_ + #warning "ODEMesh.h" +#endif + +#include +#include "ODEObject.h" + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { + namespace sim { + + /** + * The class that implements the NodeInterface interface. + * + */ + class ODEMesh : public ODEObject { + public: + ODEMesh(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEMesh(void); + bool createMesh(interfaces::NodeData *node); + }; + + } // end of namespace sim +} // end of namespace mars + +#endif // ODE_MESH_H diff --git a/sim/src/physics/ODEObject.h b/sim/src/physics/ODEObject.h index bd6d316e5..ed9cb3a7e 100644 --- a/sim/src/physics/ODEObject.h +++ b/sim/src/physics/ODEObject.h @@ -89,20 +89,14 @@ namespace mars { * The class that implements the NodeInterface interface. * */ + class ODEObject : public interfaces::NodeInterface { public: ODEObject(std::shared_ptr world, interfaces::NodeData * nodeData); virtual ~ODEObject(void); - virtual bool createNode(interfaces::NodeData *node) override { - //TODO still need switch interface here? - std::cout << "DEBUGGG: createNode in ODEObject " << __FILE__ << ":" << __LINE__ << std::endl; - return true; - }; - virtual bool changeNode(interfaces::NodeData *node) override { - std::cout << "DEBUGGG: changeNode in ODEObject " << __FILE__ << ":" << __LINE__ << std::endl; - return true; - }; + virtual bool createNode(interfaces::NodeData *node) override; + virtual bool changeNode(interfaces::NodeData *node) override; 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; @@ -158,13 +152,6 @@ namespace mars { 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); }; diff --git a/sim/src/physics/ODEObjectCreator.cpp b/sim/src/physics/ODEObjectCreator.cpp new file mode 100644 index 000000000..779dbeac9 --- /dev/null +++ b/sim/src/physics/ODEObjectCreator.cpp @@ -0,0 +1,43 @@ +#pragma once + +#include +#include +#include +#include + +namespace mars{ +namespace sim{ + +using namespace ::mars::interfaces; + +ODEObjectCreator* ODEObjectCreator::instance = NULL; + +ODEObjectCreator* ODEObjectCreator::getInstance(){ + if (NULL == instance){ + instance = new ODEObjectCreator(); + } + return instance; +} + +ODEObjectCreator::ODEObjectCreator(){ +} + +ODEObjectCreator::~ODEObjectCreator(){ +} + +std::shared_ptr ODEObjectCreator::createObject(std::shared_ptr worldPhysics, NodeData * nodeData){ + switch(nodeData->physicMode) { + case NODE_TYPE_BOX: + return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + break; + case NODE_TYPE_MESH: + return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + break; + default: + // no correct type is spezified, so no physically node will be created + return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + break; + } +} +} +} diff --git a/sim/src/physics/ODEObjectCreator.h b/sim/src/physics/ODEObjectCreator.h new file mode 100644 index 000000000..c56edaacd --- /dev/null +++ b/sim/src/physics/ODEObjectCreator.h @@ -0,0 +1,24 @@ +#pragma once + +#include + +namespace mars{ +namespace sim{ + +using namespace ::mars::interfaces; + +class ODEObjectCreator : public ODEObjectFactory{ +public: + static ODEObjectCreator* getInstance(); + +protected: + ODEObjectCreator(); + virtual ~ODEObjectCreator(); + virtual std::shared_ptr createObject(std::shared_ptr worldPhysics, NodeData * nodeData) override; + +private: + static ODEObjectCreator* instance; +}; + +} +} diff --git a/sim/src/physics/ODEObjectFactory.h b/sim/src/physics/ODEObjectFactory.h new file mode 100644 index 000000000..91df1561f --- /dev/null +++ b/sim/src/physics/ODEObjectFactory.h @@ -0,0 +1,15 @@ +#pragma once + +#include + +namespace mars{ +namespace sim{ + +using namespace ::mars::interfaces; + +class ODEObjectFactory{ +public: + virtual std::shared_ptr createObject(std::shared_ptr worldPhysics, NodeData * nodeData) = 0; +}; +} +} \ No newline at end of file diff --git a/sim/src/physics/ODESphere.cpp b/sim/src/physics/ODESphere.cpp new file mode 100644 index 000000000..637c7b54a --- /dev/null +++ b/sim/src/physics/ODESphere.cpp @@ -0,0 +1,69 @@ +/* + * 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 . + * + */ + +#include "ODESphere.h" + +namespace mars { + namespace sim { + + using namespace utils; + using namespace interfaces; + + ODESphere::ODESphere(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODESphere Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); + } + + ODESphere::~ODESphere(void) { + std::cout << "DEBUGGG: in ODESphere Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + } + + /** + * 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; + } + + } // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/ODESphere.h b/sim/src/physics/ODESphere.h new file mode 100644 index 000000000..fdb50d378 --- /dev/null +++ b/sim/src/physics/ODESphere.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 ODESphere.h + * \author Malte Roemmermann, Leon Danter + * \brief "ODESphere" implements an ODE sphere represenation for the given node + * + */ + +#ifndef ODE_SPHERE_H +#define ODE_SPHERE_H + +#ifdef _PRINT_HEADER_ + #warning "ODESphere.h" +#endif + +#include +#include "ODEObject.h" + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { + namespace sim { + + /** + * The class that implements the NodeInterface interface. + * + */ + class ODESphere : public ODEObject { + public: + ODESphere(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODESphere(void); + bool createSphere(interfaces::NodeData *node); + }; + + } // end of namespace sim +} // end of namespace mars + +#endif // ODE_SPHERE_H From 5b96bdf30a09ae97be0f486b6a043cbcfd007776 Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Fri, 12 Aug 2022 12:45:39 +0200 Subject: [PATCH 05/21] Add ODE object subclasses --- sim/CMakeLists.txt | 10 + sim/src/physics/ODEBox.cpp | 471 +++------------------------ sim/src/physics/ODEBox.h | 4 +- sim/src/physics/ODECapsule.cpp | 69 ++++ sim/src/physics/ODECapsule.h | 60 ++++ sim/src/physics/ODECylinder.cpp | 69 ++++ sim/src/physics/ODECylinder.h | 60 ++++ sim/src/physics/ODEHeightField.cpp | 91 ++++++ sim/src/physics/ODEHeightField.h | 66 ++++ sim/src/physics/ODEMesh.cpp | 16 +- sim/src/physics/ODEMesh.h | 7 +- sim/src/physics/ODEObjectCreator.cpp | 24 +- sim/src/physics/ODEPlane.cpp | 46 +++ sim/src/physics/ODEPlane.h | 60 ++++ sim/src/physics/ODESphere.cpp | 3 +- sim/src/physics/ODESphere.h | 2 +- 16 files changed, 620 insertions(+), 438 deletions(-) create mode 100644 sim/src/physics/ODECapsule.cpp create mode 100644 sim/src/physics/ODECapsule.h create mode 100644 sim/src/physics/ODECylinder.cpp create mode 100644 sim/src/physics/ODECylinder.h create mode 100644 sim/src/physics/ODEHeightField.cpp create mode 100644 sim/src/physics/ODEHeightField.h create mode 100644 sim/src/physics/ODEPlane.cpp create mode 100644 sim/src/physics/ODEPlane.h diff --git a/sim/CMakeLists.txt b/sim/CMakeLists.txt index 25b5328ae..68b1289b5 100644 --- a/sim/CMakeLists.txt +++ b/sim/CMakeLists.txt @@ -89,7 +89,12 @@ set(SOURCES_H src/physics/JointPhysics.h src/physics/ODEObject.h src/physics/ODEBox.h + src/physics/ODECapsule.h src/physics/ODEMesh.h + src/physics/ODESphere.h + src/physics/ODECylinder.h + src/physics/ODEPlane.h + src/physics/ODEHeightField.h # src/physics/NodePhysics.h src/physics/WorldPhysics.h src/physics/ContactsPhysics.hpp @@ -147,7 +152,12 @@ set(TARGET_SRC # src/physics/NodePhysics.cpp src/physics/ODEObject.cpp src/physics/ODEBox.cpp + src/physics/ODECapsule.cpp src/physics/ODEMesh.cpp + src/physics/ODESphere.cpp + src/physics/ODECylinder.cpp + src/physics/ODEPlane.cpp + src/physics/ODEHeightField.cpp src/physics/WorldPhysics.cpp src/physics/ODEObjectCreator.cpp diff --git a/sim/src/physics/ODEBox.cpp b/sim/src/physics/ODEBox.cpp index 48276100f..f0b30cb5c 100644 --- a/sim/src/physics/ODEBox.cpp +++ b/sim/src/physics/ODEBox.cpp @@ -21,441 +21,52 @@ #include "ODEBox.h" namespace mars { - namespace sim { - - using namespace utils; - using namespace interfaces; - - ODEBox::ODEBox(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODEBox Constructor " << __FILE__ << ":" << __LINE__ << std::endl; - createNode(nodeData); // pass a function pointer? - } - - ODEBox::~ODEBox(void) { - std::cout << "DEBUGGG: in ODEBox Destructor " << __FILE__ << ":" << __LINE__ << std::endl; +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEBox::ODEBox(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODEBox Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); // pass a function pointer? + } + + ODEBox::~ODEBox(void) { + std::cout << "DEBUGGG: in ODEBox Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + } + + 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; } - /** - * \brief The method creates an ode node, which properties are given by - * the NodeData param node. - * - * 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 - * - * post: - * - a physical node should be created in the world - * - the node properties should be set - */ + // build the ode representation + nGeom = dCreateBox(theWorld->getSpace(), (dReal)(node->ext.x()), + (dReal)(node->ext.y()), (dReal)(node->ext.z())); - bool ODEBox::createNode(NodeData* node) { -#ifdef _VERIFY_WORLD_ - std::cout << "DEBUGGG: in ODEBox create Node " << __FILE__ << ":" << __LINE__ << std::endl; - 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 = createBox(node); //could this be a lamda or a function pointer? - if(ret == 0) { - // Error createing the physical Node - return 0; - } - - // 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) { - 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()); - //TODO is this irrelevant as we are in ODEBox now? or is this still possible due to wrong parameter definition/ class selection - if(node->physicMode == NODE_TYPE_TERRAIN) { - dGeomGetQuaternion(nGeom, t1); - dQMultiply0(t2, tmp, t1); - dGeomSetQuaternion(nGeom, t2); - } - else - dGeomSetQuaternion(nGeom, tmp); - } - node_data.id = node->index; - dGeomSetData(nGeom, &node_data); - locker.unlock(); - setContactParams(node->c_params); - return 1; - } - LOG_WARN("ODEBox: tried to create a Box without there being a world."); - return 0; + // create the mass object for the box + if(node->inertia_set) { + setInertiaMass(node); } - - /** - * \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 - */ - //TODO Where is this called and how should it be handled?! - // Use switch for now and handle it via Registry later - bool ODEBox::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; - success = createBox(node); - 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; + else if(node->density > 0) { + dMassSetBox(&nMass, (dReal)(node->density), (dReal)(node->ext.x()), + (dReal)(node->ext.y()),(dReal)(node->ext.z())); } - - - /** - * The method creates an ode box representation of the given node. - * - */ - bool ODEBox::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; + 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())); } - -// /** -// * \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 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; -// } - } // end of namespace sim + std::cout << "Created Box!" << std::endl; + return true; + } +} // end of namespace sim } // end of namespace mars diff --git a/sim/src/physics/ODEBox.h b/sim/src/physics/ODEBox.h index fd65dac17..7a2a8b11b 100644 --- a/sim/src/physics/ODEBox.h +++ b/sim/src/physics/ODEBox.h @@ -54,9 +54,7 @@ namespace mars { //TODO createGeometry vs createCollision? nBody vs nCollision // review header comment on ODEBox - virtual bool createNode(interfaces::NodeData *node) override; //this can go to ODEObject - virtual bool changeNode(interfaces::NodeData *node) override; - bool createBox(interfaces::NodeData *node); + virtual bool createODEGeometry(interfaces::NodeData *node) override; }; } // end of namespace sim diff --git a/sim/src/physics/ODECapsule.cpp b/sim/src/physics/ODECapsule.cpp new file mode 100644 index 000000000..3526546c8 --- /dev/null +++ b/sim/src/physics/ODECapsule.cpp @@ -0,0 +1,69 @@ +/* + * 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 . + * + */ + +#include "ODECapsule.h" + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODECapsule::ODECapsule(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODECapsule Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); // pass a function pointer? + } + + ODECapsule::~ODECapsule(void) { + std::cout << "DEBUGGG: in ODECapsule Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + } + + 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()); + } + std::cout << "Created ODECapsule" << std::endl; + return true; + } +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/ODECapsule.h b/sim/src/physics/ODECapsule.h new file mode 100644 index 000000000..8b28babec --- /dev/null +++ b/sim/src/physics/ODECapsule.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 ODECapsule.h + * \author Malte Roemmermann, Leon Danter + * \brief "ODECapsule" implements an ODE capsule geometry and collision using the ODEObject class + * + */ + +#ifndef ODE_CAPSULE_H +#define ODE_CAPSULE_H + +#ifdef _PRINT_HEADER_ + #warning "ODECapsule.h" +#endif + +#include +#include "ODEObject.h" + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { + namespace sim { + + /** + * The class that implements the NodeInterface interface. + * + */ + class ODECapsule : public ODEObject { + public: + ODECapsule(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODECapsule(void); + 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/ODECylinder.cpp b/sim/src/physics/ODECylinder.cpp new file mode 100644 index 000000000..f4f01b040 --- /dev/null +++ b/sim/src/physics/ODECylinder.cpp @@ -0,0 +1,69 @@ +/* + * 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 . + * + */ + +#include "ODECylinder.h" + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODECylinder::ODECylinder(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODECylinder Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); // pass a function pointer? + } + + ODECylinder::~ODECylinder(void) { + std::cout << "DEBUGGG: in ODECylinder Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + } + + 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()); + } + std::cout << "Created ODECylinder!" << std::endl; + return true; + } +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/ODECylinder.h b/sim/src/physics/ODECylinder.h new file mode 100644 index 000000000..daeaa11c1 --- /dev/null +++ b/sim/src/physics/ODECylinder.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 ODECylinder.h + * \author Malte Roemmermann, Leon Danter + * \brief "ODECylinder" implements an ODE cylinder geometry and collision using the ODEObject class + * + */ + +#ifndef ODE_CYLINDER_H +#define ODE_CYLINDER_H + +#ifdef _PRINT_HEADER_ + #warning "ODECylinder.h" +#endif + +#include +#include "ODEObject.h" + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { + namespace sim { + + /** + * The class that implements the NodeInterface interface. + * + */ + class ODECylinder : public ODEObject { + public: + ODECylinder(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODECylinder(void); + 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/ODEHeightField.cpp b/sim/src/physics/ODEHeightField.cpp new file mode 100644 index 000000000..9572a0181 --- /dev/null +++ b/sim/src/physics/ODEHeightField.cpp @@ -0,0 +1,91 @@ +/* + * 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 . + * + */ + +#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) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODEHeightField Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + height_data = 0; + createNode(nodeData); // pass a function pointer? + } + + ODEHeightField::~ODEHeightField(void) { + std::cout << "DEBUGGG: in ODEHeightField Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + if(height_data) free(height_data); + } + + 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); + std::cout << "Created ODEHeightField!" << std::endl; + return true; + } +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/ODEHeightField.h b/sim/src/physics/ODEHeightField.h new file mode 100644 index 000000000..0f297a6e5 --- /dev/null +++ b/sim/src/physics/ODEHeightField.h @@ -0,0 +1,66 @@ +/* + * 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, Leon Danter + * \brief "ODEHeightField" implements an ODE heightfield geometry and collision using the ODEObject class + * + */ + +#ifndef ODE_HEIGHTFIELD_H +#define ODE_HEIGHTFIELD_H + +#ifdef _PRINT_HEADER_ + #warning "ODEHeightField.h" +#endif + +#include +#include "ODEObject.h" + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { + namespace sim { + + /** + * The class that implements the NodeInterface interface. + * + */ + class ODEHeightField : public ODEObject { + public: + ODEHeightField(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEHeightField(void); + 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_HEIGHTFIELD_H diff --git a/sim/src/physics/ODEMesh.cpp b/sim/src/physics/ODEMesh.cpp index 541fc41d8..c00f1bb0e 100644 --- a/sim/src/physics/ODEMesh.cpp +++ b/sim/src/physics/ODEMesh.cpp @@ -34,13 +34,26 @@ namespace mars { ODEMesh::~ODEMesh(void) { std::cout << "DEBUGGG: in ODEMesh Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + if(myVertices) free(myVertices); + if(myIndices) free(myIndices); + if(myTriMeshData) dGeomTriMeshDataDestroy(myTriMeshData); } + 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::createMesh(NodeData* node) { + bool ODEMesh::createODEGeometry(NodeData* node) { int i; if (!node->inertia_set && @@ -113,6 +126,7 @@ namespace mars { dMassSetBoxTotal(&nMass, (dReal)node->mass, (dReal)node->ext.x(), (dReal)node->ext.y(),(dReal)node->ext.z()); } + std::cout << "Created ODEMesh" << std::endl; return true; } } // end of namespace sim diff --git a/sim/src/physics/ODEMesh.h b/sim/src/physics/ODEMesh.h index 838dc8ed8..2f6c4218d 100644 --- a/sim/src/physics/ODEMesh.h +++ b/sim/src/physics/ODEMesh.h @@ -51,7 +51,12 @@ namespace mars { public: ODEMesh(std::shared_ptr world, interfaces::NodeData * nodeData); virtual ~ODEMesh(void); - bool createMesh(interfaces::NodeData *node); + virtual bool createODEGeometry(interfaces::NodeData *node) override; + virtual void destroyNode(void) override; + protected: + dVector3 *myVertices; + dTriIndex *myIndices; + dTriMeshDataID myTriMeshData; }; } // end of namespace sim diff --git a/sim/src/physics/ODEObjectCreator.cpp b/sim/src/physics/ODEObjectCreator.cpp index 779dbeac9..694ffa1c4 100644 --- a/sim/src/physics/ODEObjectCreator.cpp +++ b/sim/src/physics/ODEObjectCreator.cpp @@ -2,7 +2,13 @@ #include #include +#include +#include #include +#include +#include +#include + #include namespace mars{ @@ -30,12 +36,28 @@ std::shared_ptr ODEObjectCreator::createObject(std::shared_ptr(std::make_shared(worldPhysics, nodeData)); break; + case NODE_TYPE_CAPSULE: + return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + break; + case NODE_TYPE_CYLINDER: + return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + break; case NODE_TYPE_MESH: return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); break; + case NODE_TYPE_PLANE: + return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + break; + case NODE_TYPE_SPHERE: + return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + break; + case NODE_TYPE_TERRAIN: + return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + break; default: // no correct type is spezified, so no physically node will be created - return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + std::cout << "Unknown type of object requested " << std::endl; + return std::shared_ptr(nullptr); break; } } diff --git a/sim/src/physics/ODEPlane.cpp b/sim/src/physics/ODEPlane.cpp new file mode 100644 index 000000000..fe0140b4b --- /dev/null +++ b/sim/src/physics/ODEPlane.cpp @@ -0,0 +1,46 @@ +/* + * 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 . + * + */ + +#include "ODEPlane.h" + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEPlane::ODEPlane(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODEPlane Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); // pass a function pointer? + } + + ODEPlane::~ODEPlane(void) { + std::cout << "DEBUGGG: in ODEPlane Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + } + + bool ODEPlane::createODEGeometry(interfaces::NodeData *node){ + // build the ode representation + nGeom = dCreatePlane(theWorld->getSpace(), 0, 0, 1, (dReal)node->pos.z()); + std::cout << "Created Plane!" << std::endl; + return true; + } +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/ODEPlane.h b/sim/src/physics/ODEPlane.h new file mode 100644 index 000000000..f59b9ab2d --- /dev/null +++ b/sim/src/physics/ODEPlane.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 ODEPlane.h + * \author Malte Roemmermann, Leon Danter + * \brief "ODEPlane" implements an ODE box geometry and collision using the ODEObject class + * + */ + +#ifndef ODE_PLANE_H +#define ODE_PLANE_H + +#ifdef _PRINT_HEADER_ + #warning "ODEPlane.h" +#endif + +#include +#include "ODEObject.h" + +//TODO remove? +#ifndef ODE11 + #define dTriIndex int +#endif + +namespace mars { + namespace sim { + + /** + * The class that implements the NodeInterface interface. + * + */ + class ODEPlane : public ODEObject { + public: + ODEPlane(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEPlane(void); + 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/ODESphere.cpp b/sim/src/physics/ODESphere.cpp index 637c7b54a..dd6d49369 100644 --- a/sim/src/physics/ODESphere.cpp +++ b/sim/src/physics/ODESphere.cpp @@ -40,7 +40,7 @@ namespace mars { * The method creates an ode shpere representation of the given node. * */ - bool NodePhysics::createSphere(NodeData* 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" @@ -62,6 +62,7 @@ namespace mars { else if(node->mass > 0) { dMassSetSphereTotal(&nMass, (dReal)node->mass, (dReal)node->ext.x()); } + std::cout <<"Created ODESphere!" << std::endl; return true; } diff --git a/sim/src/physics/ODESphere.h b/sim/src/physics/ODESphere.h index fdb50d378..353c15571 100644 --- a/sim/src/physics/ODESphere.h +++ b/sim/src/physics/ODESphere.h @@ -51,7 +51,7 @@ namespace mars { public: ODESphere(std::shared_ptr world, interfaces::NodeData * nodeData); virtual ~ODESphere(void); - bool createSphere(interfaces::NodeData *node); + virtual bool createODEGeometry(interfaces::NodeData *node) override; }; } // end of namespace sim From 2067eb8c9bfbf8b06755ded75c93c616ac67cc3a Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Fri, 12 Aug 2022 12:46:23 +0200 Subject: [PATCH 06/21] Move subclass specific members to the respective subclass --- sim/src/physics/ODEObject.cpp | 177 ++++++++++++++++++++++++++++------ sim/src/physics/ODEObject.h | 7 +- 2 files changed, 148 insertions(+), 36 deletions(-) diff --git a/sim/src/physics/ODEObject.cpp b/sim/src/physics/ODEObject.cpp index 6c776b84c..0bc31e960 100644 --- a/sim/src/physics/ODEObject.cpp +++ b/sim/src/physics/ODEObject.cpp @@ -26,7 +26,6 @@ #include #include #include -#include #include #include @@ -34,6 +33,8 @@ #include +#include + namespace mars { namespace sim { @@ -61,13 +62,9 @@ namespace mars { 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); } @@ -91,10 +88,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){ @@ -104,11 +97,6 @@ namespace mars { dGeomDestroy((*iter).geom); sensor_list.erase(iter); } - if(myTriMeshData) dGeomTriMeshDataDestroy(myTriMeshData); - } - - dReal heightfield_callback(void* pUserData, int x, int z ) { - return ((ODEObject*)pUserData)->heightCallback(x, z); } /** @@ -126,6 +114,149 @@ namespace mars { * the physical position of the node * - otherwise the position should be set to zero */ + + 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", + 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; + success = createODEGeometry(node); + 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; + } + + 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); //could this be a lamda or a function pointer? + if(ret == 0) { + // Error createing the physical Node + return 0; + } + + // 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) { + 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()); + //TODO is this irrelevant as we are in ODEBox now? or is this still possible due to wrong parameter definition/ class selection + if(node->physicMode == NODE_TYPE_TERRAIN) { + dGeomGetQuaternion(nGeom, t1); + dQMultiply0(t2, tmp, t1); + dGeomSetQuaternion(nGeom, t2); + } + else + dGeomSetQuaternion(nGeom, tmp); + } + node_data.id = node->index; + dGeomSetData(nGeom, &node_data); + locker.unlock(); + setContactParams(node->c_params); + return 1; + } + LOG_WARN("ODEBox: tried to create a Box without there being a world."); + return 0; + } + + void ODEObject::getPosition(Vector* pos) const { MutexLocker locker(&(theWorld->iMutex)); if(nBody) { @@ -885,11 +1016,6 @@ namespace mars { dMassTranslate(tMass, pos[0], pos[1], pos[2]); } - dReal ODEObject::heightCallback(int x, int y) { - - return (dReal)height_data[(y*terrain->width)+x]*terrain->scale; - } - void ODEObject::setContactParams(contact_params& c_params) { MutexLocker locker(&(theWorld->iMutex)); node_data.c_params = c_params; @@ -1246,22 +1372,13 @@ namespace mars { 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 ODEObject::setInertiaMass(NodeData* node) { diff --git a/sim/src/physics/ODEObject.h b/sim/src/physics/ODEObject.h index ed9cb3a7e..ba2538fb1 100644 --- a/sim/src/physics/ODEObject.h +++ b/sim/src/physics/ODEObject.h @@ -97,6 +97,7 @@ namespace mars { 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; @@ -137,20 +138,14 @@ namespace mars { dMass getODEMass(void) const; void addMassToCompositeBody(dBodyID theBody, dMass *bodyMass); void getAbsMass(dMass *pMass) const; - dReal heightCallback(int x, int y); 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; void setProperties(interfaces::NodeData *node); void setInertiaMass(interfaces::NodeData *node); From 26f5a228fcabd9348349d963615ae3ed09e72774 Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Fri, 12 Aug 2022 14:51:36 +0200 Subject: [PATCH 07/21] Rename physics object interface classes --- sim/CMakeLists.txt | 4 ++-- sim/src/core/NodeManager.cpp | 7 ++---- sim/src/core/NodeManager.h | 4 ++-- sim/src/core/PhysicsMapper.cpp | 10 +------- sim/src/core/PhysicsMapper.h | 6 ----- sim/src/physics/ODEObjectCreator.h | 24 ------------------- ...ObjectCreator.cpp => ODEObjectFactory.cpp} | 14 +++++------ sim/src/physics/ODEObjectFactory.h | 17 +++++++++---- sim/src/physics/ObjectFactoryInterface.h | 15 ++++++++++++ 9 files changed, 42 insertions(+), 59 deletions(-) delete mode 100644 sim/src/physics/ODEObjectCreator.h rename sim/src/physics/{ODEObjectCreator.cpp => ODEObjectFactory.cpp} (84%) create mode 100644 sim/src/physics/ObjectFactoryInterface.h diff --git a/sim/CMakeLists.txt b/sim/CMakeLists.txt index 68b1289b5..b7ae6ea8c 100644 --- a/sim/CMakeLists.txt +++ b/sim/CMakeLists.txt @@ -98,8 +98,8 @@ set(SOURCES_H # src/physics/NodePhysics.h src/physics/WorldPhysics.h src/physics/ContactsPhysics.hpp - src/physics/ODEObjectCreator.h src/physics/ODEObjectFactory.h + src/physics/ObjectFactoryInterface.h src/sensors/CameraSensor.h src/sensors/IDListConfig.h @@ -159,7 +159,7 @@ set(TARGET_SRC src/physics/ODEPlane.cpp src/physics/ODEHeightField.cpp src/physics/WorldPhysics.cpp - src/physics/ODEObjectCreator.cpp + src/physics/ODEObjectFactory.cpp src/sensors/CameraSensor.cpp src/sensors/Joint6DOFSensor.cpp diff --git a/sim/src/core/NodeManager.cpp b/sim/src/core/NodeManager.cpp index e4d516b83..026209c2f 100644 --- a/sim/src/core/NodeManager.cpp +++ b/sim/src/core/NodeManager.cpp @@ -75,8 +75,7 @@ namespace mars { GraphicsUpdateInterface *gui = static_cast(this); control->graphics->addGraphicsUpdateInterface(gui); } - std::cout << "Here 1" << std::endl; - odeObjectFactory = ODEObjectCreator::getInstance(); + odeObjectFactory = ODEObjectFactory::getInstance(); } NodeManager::~NodeManager() @@ -251,10 +250,8 @@ namespace mars { // create the physical node data if(! (nodeS->noPhysical)){ // create an interface object to the physics - //std::shared_ptr newNodeInterface = PhysicsMapper::newODEObject(control->sim->getPhysics(), nodeS); std::shared_ptr newNodeInterface = odeObjectFactory->createObject(control->sim->getPhysics(), nodeS); - - std::cout << "DEBUGGG: create ODEObject pointer with getPhyiscs NodeManager " << __FILE__ << ":" << __LINE__ << std::endl; + std::cout << "DEBUGGG: create ODEObject pointer with odeObjectFactory in NodeManager " << __FILE__ << ":" << __LINE__ << std::endl; if (newNodeInterface.get() == nullptr) { // if no node was created in physics diff --git a/sim/src/core/NodeManager.h b/sim/src/core/NodeManager.h index 1a63ae09e..719b38470 100644 --- a/sim/src/core/NodeManager.h +++ b/sim/src/core/NodeManager.h @@ -37,8 +37,8 @@ #include #include +#include #include -#include namespace mars { namespace sim { @@ -166,7 +166,7 @@ namespace mars { private: - ODEObjectFactory* odeObjectFactory; + ObjectFactoryInterface* odeObjectFactory; interfaces::NodeId next_node_id; bool update_all_nodes; diff --git a/sim/src/core/PhysicsMapper.cpp b/sim/src/core/PhysicsMapper.cpp index c0579e9fd..e493761d5 100644 --- a/sim/src/core/PhysicsMapper.cpp +++ b/sim/src/core/PhysicsMapper.cpp @@ -33,19 +33,11 @@ namespace mars { 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::newODEObject(std::shared_ptr worldPhysics, NodeData * nodeData) { - // Create a nodePhysics with the worldPhysics as constructor parameter, then downcast to a Node interface and return - std::shared_ptr nodeInterface; - // first we create a ode geometry for the node - return nodeInterface; - - } - std::shared_ptr PhysicsMapper::newJointPhysics(std::shared_ptr worldPhysics) { std::shared_ptr jointPhysics = std::make_shared(worldPhysics); return std::static_pointer_cast(jointPhysics); diff --git a/sim/src/core/PhysicsMapper.h b/sim/src/core/PhysicsMapper.h index 9db16de02..0c41bc72b 100644 --- a/sim/src/core/PhysicsMapper.h +++ b/sim/src/core/PhysicsMapper.h @@ -34,8 +34,6 @@ #endif #include "WorldPhysics.h" -#include "ODEBox.h" -#include "ODEObject.h" #include "JointPhysics.h" namespace mars { @@ -44,11 +42,7 @@ namespace mars { class PhysicsMapper { public: static std::shared_ptr newWorldPhysics(interfaces::ControlCenter *control); - //TODO naming. Would prefer ODEObject here - // check other files if NodeInterface pointer would be preferable over ODEObject pointer? - static std::shared_ptr newODEObject(std::shared_ptr worldPhysics, interfaces::NodeData * nodeData); static std::shared_ptr newJointPhysics(std::shared_ptr worldPhysics); - }; } // end of namespace sim diff --git a/sim/src/physics/ODEObjectCreator.h b/sim/src/physics/ODEObjectCreator.h deleted file mode 100644 index c56edaacd..000000000 --- a/sim/src/physics/ODEObjectCreator.h +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -#include - -namespace mars{ -namespace sim{ - -using namespace ::mars::interfaces; - -class ODEObjectCreator : public ODEObjectFactory{ -public: - static ODEObjectCreator* getInstance(); - -protected: - ODEObjectCreator(); - virtual ~ODEObjectCreator(); - virtual std::shared_ptr createObject(std::shared_ptr worldPhysics, NodeData * nodeData) override; - -private: - static ODEObjectCreator* instance; -}; - -} -} diff --git a/sim/src/physics/ODEObjectCreator.cpp b/sim/src/physics/ODEObjectFactory.cpp similarity index 84% rename from sim/src/physics/ODEObjectCreator.cpp rename to sim/src/physics/ODEObjectFactory.cpp index 694ffa1c4..78ed1f1c5 100644 --- a/sim/src/physics/ODEObjectCreator.cpp +++ b/sim/src/physics/ODEObjectFactory.cpp @@ -1,6 +1,6 @@ #pragma once -#include +#include #include #include #include @@ -16,22 +16,22 @@ namespace sim{ using namespace ::mars::interfaces; -ODEObjectCreator* ODEObjectCreator::instance = NULL; +ODEObjectFactory* ODEObjectFactory::instance = NULL; -ODEObjectCreator* ODEObjectCreator::getInstance(){ +ODEObjectFactory* ODEObjectFactory::getInstance(){ if (NULL == instance){ - instance = new ODEObjectCreator(); + instance = new ODEObjectFactory(); } return instance; } -ODEObjectCreator::ODEObjectCreator(){ +ODEObjectFactory::ODEObjectFactory(){ } -ODEObjectCreator::~ODEObjectCreator(){ +ODEObjectFactory::~ODEObjectFactory(){ } -std::shared_ptr ODEObjectCreator::createObject(std::shared_ptr worldPhysics, NodeData * nodeData){ +std::shared_ptr ODEObjectFactory::createObject(std::shared_ptr worldPhysics, NodeData * nodeData){ switch(nodeData->physicMode) { case NODE_TYPE_BOX: return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); diff --git a/sim/src/physics/ODEObjectFactory.h b/sim/src/physics/ODEObjectFactory.h index 91df1561f..36a18344c 100644 --- a/sim/src/physics/ODEObjectFactory.h +++ b/sim/src/physics/ODEObjectFactory.h @@ -1,15 +1,24 @@ #pragma once -#include +#include namespace mars{ namespace sim{ using namespace ::mars::interfaces; -class ODEObjectFactory{ +class ODEObjectFactory : public ObjectFactoryInterface{ public: - virtual std::shared_ptr createObject(std::shared_ptr worldPhysics, NodeData * nodeData) = 0; + static ODEObjectFactory* getInstance(); + +protected: + ODEObjectFactory(); + virtual ~ODEObjectFactory(); + virtual std::shared_ptr createObject(std::shared_ptr worldPhysics, NodeData * nodeData) override; + +private: + static ODEObjectFactory* instance; }; + +} } -} \ No newline at end of file diff --git a/sim/src/physics/ObjectFactoryInterface.h b/sim/src/physics/ObjectFactoryInterface.h new file mode 100644 index 000000000..5b3b63ec3 --- /dev/null +++ b/sim/src/physics/ObjectFactoryInterface.h @@ -0,0 +1,15 @@ +#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 From c6163626a60fa583e58833206f364bbc2ed92f28 Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Fri, 12 Aug 2022 15:08:18 +0200 Subject: [PATCH 08/21] Add and update license information --- sim/src/physics/ODEBox.cpp | 2 +- sim/src/physics/ODECapsule.cpp | 2 +- sim/src/physics/ODECylinder.cpp | 2 +- sim/src/physics/ODEHeightField.cpp | 2 +- sim/src/physics/ODEMesh.cpp | 2 +- sim/src/physics/ODEMesh.h | 2 +- sim/src/physics/ODEObject.cpp | 2 +- sim/src/physics/ODEObject.h | 4 ++-- sim/src/physics/ODEObjectFactory.cpp | 20 +++++++++++++++++- sim/src/physics/ODEObjectFactory.h | 27 ++++++++++++++++++++++++ sim/src/physics/ODEPlane.cpp | 2 +- sim/src/physics/ODEPlane.h | 4 ++-- sim/src/physics/ODESphere.cpp | 2 +- sim/src/physics/ODESphere.h | 2 +- sim/src/physics/ObjectFactoryInterface.h | 27 ++++++++++++++++++++++++ 15 files changed, 87 insertions(+), 15 deletions(-) diff --git a/sim/src/physics/ODEBox.cpp b/sim/src/physics/ODEBox.cpp index f0b30cb5c..f38f86c6c 100644 --- a/sim/src/physics/ODEBox.cpp +++ b/sim/src/physics/ODEBox.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. * diff --git a/sim/src/physics/ODECapsule.cpp b/sim/src/physics/ODECapsule.cpp index 3526546c8..ce50fdf0d 100644 --- a/sim/src/physics/ODECapsule.cpp +++ b/sim/src/physics/ODECapsule.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. * diff --git a/sim/src/physics/ODECylinder.cpp b/sim/src/physics/ODECylinder.cpp index f4f01b040..1a92b833a 100644 --- a/sim/src/physics/ODECylinder.cpp +++ b/sim/src/physics/ODECylinder.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. * diff --git a/sim/src/physics/ODEHeightField.cpp b/sim/src/physics/ODEHeightField.cpp index 9572a0181..4c200cab1 100644 --- a/sim/src/physics/ODEHeightField.cpp +++ b/sim/src/physics/ODEHeightField.cpp @@ -1,5 +1,5 @@ /* - * Copyright 2011, 2012, DFKI GmbH Robotics Innovation Center + * Copyright 2022, 2012, DFKI GmbH Robotics Innovation Center * * This file is part of the MARS simulation framework. * diff --git a/sim/src/physics/ODEMesh.cpp b/sim/src/physics/ODEMesh.cpp index c00f1bb0e..88fe21511 100644 --- a/sim/src/physics/ODEMesh.cpp +++ b/sim/src/physics/ODEMesh.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. * diff --git a/sim/src/physics/ODEMesh.h b/sim/src/physics/ODEMesh.h index 2f6c4218d..ad1469544 100644 --- a/sim/src/physics/ODEMesh.h +++ b/sim/src/physics/ODEMesh.h @@ -20,7 +20,7 @@ /** * \file ODEMesh.h - * \author Malte Roemmermann, Leon Danter + * \author Muhammad Haider Khan Lodhi * \brief "ODEMesh" implements an ode mesh representation of the given node. * */ diff --git a/sim/src/physics/ODEObject.cpp b/sim/src/physics/ODEObject.cpp index 0bc31e960..2d0de6a2e 100644 --- a/sim/src/physics/ODEObject.cpp +++ b/sim/src/physics/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. * diff --git a/sim/src/physics/ODEObject.h b/sim/src/physics/ODEObject.h index ba2538fb1..34f5cd695 100644 --- a/sim/src/physics/ODEObject.h +++ b/sim/src/physics/ODEObject.h @@ -20,8 +20,8 @@ /** * \file ODEObject.h - * \author Malte Roemmermann, Leon Danter - * \brief "ODEObject" implements a factory to create physical ode objects for the nodes. + * \author Malte Roemmermann, Leon Danter, Muhammad Haider Khan Lodhi + * \brief "ODEObject" implements an ODEObject as parent of ode objects. * */ diff --git a/sim/src/physics/ODEObjectFactory.cpp b/sim/src/physics/ODEObjectFactory.cpp index 78ed1f1c5..8f40507d9 100644 --- a/sim/src/physics/ODEObjectFactory.cpp +++ b/sim/src/physics/ODEObjectFactory.cpp @@ -1,4 +1,22 @@ -#pragma once +/* + * 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 #include diff --git a/sim/src/physics/ODEObjectFactory.h b/sim/src/physics/ODEObjectFactory.h index 36a18344c..1832b87b3 100644 --- a/sim/src/physics/ODEObjectFactory.h +++ b/sim/src/physics/ODEObjectFactory.h @@ -1,3 +1,30 @@ +/* + * 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 diff --git a/sim/src/physics/ODEPlane.cpp b/sim/src/physics/ODEPlane.cpp index fe0140b4b..a0807b83c 100644 --- a/sim/src/physics/ODEPlane.cpp +++ b/sim/src/physics/ODEPlane.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. * diff --git a/sim/src/physics/ODEPlane.h b/sim/src/physics/ODEPlane.h index f59b9ab2d..cd3f15104 100644 --- a/sim/src/physics/ODEPlane.h +++ b/sim/src/physics/ODEPlane.h @@ -20,8 +20,8 @@ /** * \file ODEPlane.h - * \author Malte Roemmermann, Leon Danter - * \brief "ODEPlane" implements an ODE box geometry and collision using the ODEObject class + * \author Muhammad Haider Khan Lodhi + * \brief "ODEPlane" implements an ODE plane geometry and collision using the ODEObject class * */ diff --git a/sim/src/physics/ODESphere.cpp b/sim/src/physics/ODESphere.cpp index dd6d49369..dc345599f 100644 --- a/sim/src/physics/ODESphere.cpp +++ b/sim/src/physics/ODESphere.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. * diff --git a/sim/src/physics/ODESphere.h b/sim/src/physics/ODESphere.h index 353c15571..0debfc0a3 100644 --- a/sim/src/physics/ODESphere.h +++ b/sim/src/physics/ODESphere.h @@ -20,7 +20,7 @@ /** * \file ODESphere.h - * \author Malte Roemmermann, Leon Danter + * \author Muhammad Haider Khan Lodhi * \brief "ODESphere" implements an ODE sphere represenation for the given node * */ diff --git a/sim/src/physics/ObjectFactoryInterface.h b/sim/src/physics/ObjectFactoryInterface.h index 5b3b63ec3..157798674 100644 --- a/sim/src/physics/ObjectFactoryInterface.h +++ b/sim/src/physics/ObjectFactoryInterface.h @@ -1,3 +1,30 @@ +/* + * 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 From b8c693a21582df263b6f0ccaa0f09311eeee918e Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Mon, 15 Aug 2022 14:35:50 +0200 Subject: [PATCH 09/21] Add CoreODEObjects and use singelton for the concrete factory --- sim/CMakeLists.txt | 20 +- sim/src/core/NodeManager.cpp | 9 +- sim/src/core/NodeManager.h | 5 - sim/src/physics/CoreODEObjects.cpp | 377 +++++++++++++++++++++++++++ sim/src/physics/CoreODEObjects.h | 112 ++++++++ sim/src/physics/ODEBox.cpp | 72 ----- sim/src/physics/ODEBox.h | 63 ----- sim/src/physics/ODECapsule.cpp | 69 ----- sim/src/physics/ODECapsule.h | 60 ----- sim/src/physics/ODECylinder.cpp | 69 ----- sim/src/physics/ODECylinder.h | 60 ----- sim/src/physics/ODEHeightField.cpp | 91 ------- sim/src/physics/ODEHeightField.h | 66 ----- sim/src/physics/ODEMesh.cpp | 133 ---------- sim/src/physics/ODEMesh.h | 65 ----- sim/src/physics/ODEObjectFactory.cpp | 30 +-- sim/src/physics/ODEObjectFactory.h | 7 +- sim/src/physics/ODEPlane.cpp | 46 ---- sim/src/physics/ODEPlane.h | 60 ----- sim/src/physics/ODESphere.cpp | 70 ----- sim/src/physics/ODESphere.h | 60 ----- 21 files changed, 509 insertions(+), 1035 deletions(-) create mode 100644 sim/src/physics/CoreODEObjects.cpp create mode 100644 sim/src/physics/CoreODEObjects.h delete mode 100644 sim/src/physics/ODEBox.cpp delete mode 100644 sim/src/physics/ODEBox.h delete mode 100644 sim/src/physics/ODECapsule.cpp delete mode 100644 sim/src/physics/ODECapsule.h delete mode 100644 sim/src/physics/ODECylinder.cpp delete mode 100644 sim/src/physics/ODECylinder.h delete mode 100644 sim/src/physics/ODEHeightField.cpp delete mode 100644 sim/src/physics/ODEHeightField.h delete mode 100644 sim/src/physics/ODEMesh.cpp delete mode 100644 sim/src/physics/ODEMesh.h delete mode 100644 sim/src/physics/ODEPlane.cpp delete mode 100644 sim/src/physics/ODEPlane.h delete mode 100644 sim/src/physics/ODESphere.cpp delete mode 100644 sim/src/physics/ODESphere.h diff --git a/sim/CMakeLists.txt b/sim/CMakeLists.txt index b7ae6ea8c..47124ef69 100644 --- a/sim/CMakeLists.txt +++ b/sim/CMakeLists.txt @@ -88,18 +88,11 @@ set(SOURCES_H src/physics/JointPhysics.h src/physics/ODEObject.h - src/physics/ODEBox.h - src/physics/ODECapsule.h - src/physics/ODEMesh.h - src/physics/ODESphere.h - src/physics/ODECylinder.h - src/physics/ODEPlane.h - src/physics/ODEHeightField.h -# src/physics/NodePhysics.h src/physics/WorldPhysics.h src/physics/ContactsPhysics.hpp src/physics/ODEObjectFactory.h src/physics/ObjectFactoryInterface.h + src/physics/CoreODEObjects.h src/sensors/CameraSensor.h src/sensors/IDListConfig.h @@ -149,18 +142,11 @@ set(TARGET_SRC src/sensors/RotatingRaySensor.cpp src/physics/JointPhysics.cpp - # src/physics/NodePhysics.cpp src/physics/ODEObject.cpp - src/physics/ODEBox.cpp - src/physics/ODECapsule.cpp - src/physics/ODEMesh.cpp - src/physics/ODESphere.cpp - src/physics/ODECylinder.cpp - src/physics/ODEPlane.cpp - src/physics/ODEHeightField.cpp src/physics/WorldPhysics.cpp src/physics/ODEObjectFactory.cpp - + src/physics/CoreODEObjects.cpp + src/sensors/CameraSensor.cpp src/sensors/Joint6DOFSensor.cpp src/sensors/JointArraySensor.cpp diff --git a/sim/src/core/NodeManager.cpp b/sim/src/core/NodeManager.cpp index 026209c2f..1d29b36f3 100644 --- a/sim/src/core/NodeManager.cpp +++ b/sim/src/core/NodeManager.cpp @@ -47,6 +47,9 @@ #include +#include +#include + namespace mars { namespace sim { @@ -75,16 +78,12 @@ namespace mars { GraphicsUpdateInterface *gui = static_cast(this); control->graphics->addGraphicsUpdateInterface(gui); } - odeObjectFactory = ODEObjectFactory::getInstance(); } NodeManager::~NodeManager() { - std::cout << "Destroyed " << std::endl; - delete odeObjectFactory; } - NodeId NodeManager::createPrimitiveNode(const std::string &name, NodeType type, bool moveable, @@ -250,7 +249,7 @@ namespace mars { // create the physical node data if(! (nodeS->noPhysical)){ // create an interface object to the physics - std::shared_ptr newNodeInterface = odeObjectFactory->createObject(control->sim->getPhysics(), nodeS); + std::shared_ptr newNodeInterface = ODEObjectFactory::Instance().createObject(control->sim->getPhysics(), nodeS); std::cout << "DEBUGGG: create ODEObject pointer with odeObjectFactory in NodeManager " << __FILE__ << ":" << __LINE__ << std::endl; if (newNodeInterface.get() == nullptr) { diff --git a/sim/src/core/NodeManager.h b/sim/src/core/NodeManager.h index 719b38470..4b4398f2a 100644 --- a/sim/src/core/NodeManager.h +++ b/sim/src/core/NodeManager.h @@ -37,9 +37,6 @@ #include #include -#include -#include - namespace mars { namespace sim { @@ -166,8 +163,6 @@ namespace mars { private: - ObjectFactoryInterface* odeObjectFactory; - interfaces::NodeId next_node_id; bool update_all_nodes; int visual_rep; diff --git a/sim/src/physics/CoreODEObjects.cpp b/sim/src/physics/CoreODEObjects.cpp new file mode 100644 index 000000000..72166056d --- /dev/null +++ b/sim/src/physics/CoreODEObjects.cpp @@ -0,0 +1,377 @@ +/* + * 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 "CoreODEObjects.h" +#include + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + ODEBox::ODEBox(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODEBox Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); // pass a function pointer? + } + + ODEBox::~ODEBox(void) { + std::cout << "DEBUGGG: in ODEBox Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + } + + 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())); + } + + std::cout << "Created Box!" << std::endl; + return true; + } + + ODECapsule::ODECapsule(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODECapsule Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); // pass a function pointer? + } + + ODECapsule::~ODECapsule(void) { + std::cout << "DEBUGGG: in ODECapsule Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + } + + 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()); + } + std::cout << "Created ODECapsule" << std::endl; + return true; + } + + ODECylinder::ODECylinder(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODECylinder Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); // pass a function pointer? + } + + ODECylinder::~ODECylinder(void) { + std::cout << "DEBUGGG: in ODECylinder Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + } + + 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()); + } + std::cout << "Created ODECylinder!" << std::endl; + return true; + } + + ODEHeightField::ODEHeightField(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODEHeightField Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + height_data = 0; + createNode(nodeData); // pass a function pointer? + } + + ODEHeightField::~ODEHeightField(void) { + std::cout << "DEBUGGG: in ODEHeightField Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + if(height_data) free(height_data); + if(terrain) free (terrain); + } + + 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); + std::cout << "Created ODEHeightField!" << std::endl; + return true; + } + + ODEMesh::ODEMesh(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODEMesh Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); + } + + ODEMesh::~ODEMesh(void) { + std::cout << "DEBUGGG: in ODEMesh Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + if(myVertices) free(myVertices); + if(myIndices) free(myIndices); + if(myTriMeshData) dGeomTriMeshDataDestroy(myTriMeshData); + } + + 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()); + } + std::cout << "Created ODEMesh" << std::endl; + return true; + } + + ODEPlane::ODEPlane(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODEPlane Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); // pass a function pointer? + } + + ODEPlane::~ODEPlane(void) { + std::cout << "DEBUGGG: in ODEPlane Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + } + + bool ODEPlane::createODEGeometry(interfaces::NodeData *node){ + // build the ode representation + nGeom = dCreatePlane(theWorld->getSpace(), 0, 0, 1, (dReal)node->pos.z()); + std::cout << "Created Plane!" << std::endl; + return true; + } + + ODESphere::ODESphere(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { + // At this moment we have not much things to do here. ^_^ + std::cout << "DEBUGGG: in ODESphere Constructor " << __FILE__ << ":" << __LINE__ << std::endl; + createNode(nodeData); + } + + ODESphere::~ODESphere(void) { + std::cout << "DEBUGGG: in ODESphere Destructor " << __FILE__ << ":" << __LINE__ << std::endl; + } + + /** + * 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()); + } + std::cout <<"Created ODESphere!" << std::endl; + return true; + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/CoreODEObjects.h b/sim/src/physics/CoreODEObjects.h new file mode 100644 index 000000000..bf1946b35 --- /dev/null +++ b/sim/src/physics/CoreODEObjects.h @@ -0,0 +1,112 @@ +/* + * 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 CoreODEObjects.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * \brief A collection of OBE object classes + * + */ + +#ifndef CORE_ODE_OBJECTS_H +#define CORE_ODE_OBJECTS_H + +#ifdef _PRINT_HEADER_ + #warning "CoreODEObjects.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); + //TODO createGeometry vs createCollision? nBody vs nCollision + // review header comment on ODEBox + virtual bool createODEGeometry(interfaces::NodeData *node) override; + }; + + class ODECapsule : public ODEObject { + public: + ODECapsule(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODECapsule(void); + virtual bool createODEGeometry(interfaces::NodeData *node) override; + }; + + class ODECylinder : public ODEObject { + public: + ODECylinder(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODECylinder(void); + virtual bool createODEGeometry(interfaces::NodeData *node) override; + }; + + class ODEHeightField : public ODEObject { + public: + ODEHeightField(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEHeightField(void); + 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; + }; + + class ODEMesh : public ODEObject { + public: + ODEMesh(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEMesh(void); + virtual bool createODEGeometry(interfaces::NodeData *node) override; + virtual void destroyNode(void) override; + protected: + dVector3 *myVertices; + dTriIndex *myIndices; + dTriMeshDataID myTriMeshData; + }; + + class ODEPlane : public ODEObject { + public: + ODEPlane(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODEPlane(void); + virtual bool createODEGeometry(interfaces::NodeData *node) override; + }; + + class ODESphere : public ODEObject { + public: + ODESphere(std::shared_ptr world, interfaces::NodeData * nodeData); + virtual ~ODESphere(void); + virtual bool createODEGeometry(interfaces::NodeData *node) override; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // CORE_ODE_OBJECTS_H diff --git a/sim/src/physics/ODEBox.cpp b/sim/src/physics/ODEBox.cpp deleted file mode 100644 index f38f86c6c..000000000 --- a/sim/src/physics/ODEBox.cpp +++ /dev/null @@ -1,72 +0,0 @@ -/* - * 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" - -namespace mars { -namespace sim { - - using namespace utils; - using namespace interfaces; - - ODEBox::ODEBox(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODEBox Constructor " << __FILE__ << ":" << __LINE__ << std::endl; - createNode(nodeData); // pass a function pointer? - } - - ODEBox::~ODEBox(void) { - std::cout << "DEBUGGG: in ODEBox Destructor " << __FILE__ << ":" << __LINE__ << std::endl; - } - - 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())); - } - - std::cout << "Created Box!" << std::endl; - return true; - } -} // end of namespace sim -} // end of namespace mars diff --git a/sim/src/physics/ODEBox.h b/sim/src/physics/ODEBox.h deleted file mode 100644 index 7a2a8b11b..000000000 --- a/sim/src/physics/ODEBox.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * 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, Leon Danter - * \brief "ODEBox" implements an ODE box geometry and collision using the ODEObject class - * - */ - -#ifndef ODE_BOX_H -#define ODE_BOX_H - -#ifdef _PRINT_HEADER_ - #warning "ODEBox.h" -#endif - -#include -#include "ODEObject.h" - -//TODO remove? -#ifndef ODE11 - #define dTriIndex int -#endif - -namespace mars { - namespace sim { - - /** - * The class that implements the NodeInterface interface. - * - */ - class ODEBox : public ODEObject { - public: - ODEBox(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODEBox(void); - - //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/ODECapsule.cpp b/sim/src/physics/ODECapsule.cpp deleted file mode 100644 index ce50fdf0d..000000000 --- a/sim/src/physics/ODECapsule.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* - * 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" - -namespace mars { -namespace sim { - - using namespace utils; - using namespace interfaces; - - ODECapsule::ODECapsule(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODECapsule Constructor " << __FILE__ << ":" << __LINE__ << std::endl; - createNode(nodeData); // pass a function pointer? - } - - ODECapsule::~ODECapsule(void) { - std::cout << "DEBUGGG: in ODECapsule Destructor " << __FILE__ << ":" << __LINE__ << std::endl; - } - - 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()); - } - std::cout << "Created ODECapsule" << std::endl; - return true; - } -} // end of namespace sim -} // end of namespace mars diff --git a/sim/src/physics/ODECapsule.h b/sim/src/physics/ODECapsule.h deleted file mode 100644 index 8b28babec..000000000 --- a/sim/src/physics/ODECapsule.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * 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, Leon Danter - * \brief "ODECapsule" implements an ODE capsule geometry and collision using the ODEObject class - * - */ - -#ifndef ODE_CAPSULE_H -#define ODE_CAPSULE_H - -#ifdef _PRINT_HEADER_ - #warning "ODECapsule.h" -#endif - -#include -#include "ODEObject.h" - -//TODO remove? -#ifndef ODE11 - #define dTriIndex int -#endif - -namespace mars { - namespace sim { - - /** - * The class that implements the NodeInterface interface. - * - */ - class ODECapsule : public ODEObject { - public: - ODECapsule(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODECapsule(void); - 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/ODECylinder.cpp b/sim/src/physics/ODECylinder.cpp deleted file mode 100644 index 1a92b833a..000000000 --- a/sim/src/physics/ODECylinder.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* - * 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" - -namespace mars { -namespace sim { - - using namespace utils; - using namespace interfaces; - - ODECylinder::ODECylinder(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODECylinder Constructor " << __FILE__ << ":" << __LINE__ << std::endl; - createNode(nodeData); // pass a function pointer? - } - - ODECylinder::~ODECylinder(void) { - std::cout << "DEBUGGG: in ODECylinder Destructor " << __FILE__ << ":" << __LINE__ << std::endl; - } - - 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()); - } - std::cout << "Created ODECylinder!" << std::endl; - return true; - } -} // end of namespace sim -} // end of namespace mars diff --git a/sim/src/physics/ODECylinder.h b/sim/src/physics/ODECylinder.h deleted file mode 100644 index daeaa11c1..000000000 --- a/sim/src/physics/ODECylinder.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * 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, Leon Danter - * \brief "ODECylinder" implements an ODE cylinder geometry and collision using the ODEObject class - * - */ - -#ifndef ODE_CYLINDER_H -#define ODE_CYLINDER_H - -#ifdef _PRINT_HEADER_ - #warning "ODECylinder.h" -#endif - -#include -#include "ODEObject.h" - -//TODO remove? -#ifndef ODE11 - #define dTriIndex int -#endif - -namespace mars { - namespace sim { - - /** - * The class that implements the NodeInterface interface. - * - */ - class ODECylinder : public ODEObject { - public: - ODECylinder(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODECylinder(void); - 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/ODEHeightField.cpp b/sim/src/physics/ODEHeightField.cpp deleted file mode 100644 index 4c200cab1..000000000 --- a/sim/src/physics/ODEHeightField.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/* - * Copyright 2022, 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 . - * - */ - -#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) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODEHeightField Constructor " << __FILE__ << ":" << __LINE__ << std::endl; - height_data = 0; - createNode(nodeData); // pass a function pointer? - } - - ODEHeightField::~ODEHeightField(void) { - std::cout << "DEBUGGG: in ODEHeightField Destructor " << __FILE__ << ":" << __LINE__ << std::endl; - if(height_data) free(height_data); - } - - 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); - std::cout << "Created ODEHeightField!" << std::endl; - return true; - } -} // end of namespace sim -} // end of namespace mars diff --git a/sim/src/physics/ODEHeightField.h b/sim/src/physics/ODEHeightField.h deleted file mode 100644 index 0f297a6e5..000000000 --- a/sim/src/physics/ODEHeightField.h +++ /dev/null @@ -1,66 +0,0 @@ -/* - * 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, Leon Danter - * \brief "ODEHeightField" implements an ODE heightfield geometry and collision using the ODEObject class - * - */ - -#ifndef ODE_HEIGHTFIELD_H -#define ODE_HEIGHTFIELD_H - -#ifdef _PRINT_HEADER_ - #warning "ODEHeightField.h" -#endif - -#include -#include "ODEObject.h" - -//TODO remove? -#ifndef ODE11 - #define dTriIndex int -#endif - -namespace mars { - namespace sim { - - /** - * The class that implements the NodeInterface interface. - * - */ - class ODEHeightField : public ODEObject { - public: - ODEHeightField(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODEHeightField(void); - 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_HEIGHTFIELD_H diff --git a/sim/src/physics/ODEMesh.cpp b/sim/src/physics/ODEMesh.cpp deleted file mode 100644 index 88fe21511..000000000 --- a/sim/src/physics/ODEMesh.cpp +++ /dev/null @@ -1,133 +0,0 @@ -/* - * 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" - -namespace mars { - namespace sim { - - using namespace utils; - using namespace interfaces; - - ODEMesh::ODEMesh(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODEMesh Constructor " << __FILE__ << ":" << __LINE__ << std::endl; - createNode(nodeData); - } - - ODEMesh::~ODEMesh(void) { - std::cout << "DEBUGGG: in ODEMesh Destructor " << __FILE__ << ":" << __LINE__ << std::endl; - if(myVertices) free(myVertices); - if(myIndices) free(myIndices); - if(myTriMeshData) dGeomTriMeshDataDestroy(myTriMeshData); - } - - 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()); - } - std::cout << "Created ODEMesh" << std::endl; - return true; - } - } // end of namespace sim -} // end of namespace mars diff --git a/sim/src/physics/ODEMesh.h b/sim/src/physics/ODEMesh.h deleted file mode 100644 index ad1469544..000000000 --- a/sim/src/physics/ODEMesh.h +++ /dev/null @@ -1,65 +0,0 @@ -/* - * 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 Muhammad Haider Khan Lodhi - * \brief "ODEMesh" implements an ode mesh representation of the given node. - * - */ - -#ifndef ODE_MESH_H -#define ODE_MESH_H - -#ifdef _PRINT_HEADER_ - #warning "ODEMesh.h" -#endif - -#include -#include "ODEObject.h" - -//TODO remove? -#ifndef ODE11 - #define dTriIndex int -#endif - -namespace mars { - namespace sim { - - /** - * The class that implements the NodeInterface interface. - * - */ - class ODEMesh : public ODEObject { - public: - ODEMesh(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODEMesh(void); - 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/ODEObjectFactory.cpp b/sim/src/physics/ODEObjectFactory.cpp index 8f40507d9..67a07a452 100644 --- a/sim/src/physics/ODEObjectFactory.cpp +++ b/sim/src/physics/ODEObjectFactory.cpp @@ -19,13 +19,7 @@ */ #include -#include -#include -#include -#include -#include -#include -#include +#include #include @@ -34,12 +28,10 @@ namespace sim{ using namespace ::mars::interfaces; -ODEObjectFactory* ODEObjectFactory::instance = NULL; +//ODEObjectFactory* ODEObjectFactory::instance = NULL; -ODEObjectFactory* ODEObjectFactory::getInstance(){ - if (NULL == instance){ - instance = new ODEObjectFactory(); - } +ODEObjectFactory& ODEObjectFactory::Instance(){ + static ODEObjectFactory instance; return instance; } @@ -52,25 +44,25 @@ ODEObjectFactory::~ODEObjectFactory(){ std::shared_ptr ODEObjectFactory::createObject(std::shared_ptr worldPhysics, NodeData * nodeData){ switch(nodeData->physicMode) { case NODE_TYPE_BOX: - return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + return std::make_shared(worldPhysics, nodeData); break; case NODE_TYPE_CAPSULE: - return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + return std::make_shared(worldPhysics, nodeData); break; case NODE_TYPE_CYLINDER: - return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + return std::make_shared(worldPhysics, nodeData); break; case NODE_TYPE_MESH: - return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + return std::make_shared(worldPhysics, nodeData); break; case NODE_TYPE_PLANE: - return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + return std::make_shared(worldPhysics, nodeData); break; case NODE_TYPE_SPHERE: - return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + return std::make_shared(worldPhysics, nodeData); break; case NODE_TYPE_TERRAIN: - return std::static_pointer_cast(std::make_shared(worldPhysics, nodeData)); + return std::make_shared(worldPhysics, nodeData); break; default: // no correct type is spezified, so no physically node will be created diff --git a/sim/src/physics/ODEObjectFactory.h b/sim/src/physics/ODEObjectFactory.h index 1832b87b3..9df2406fc 100644 --- a/sim/src/physics/ODEObjectFactory.h +++ b/sim/src/physics/ODEObjectFactory.h @@ -36,15 +36,12 @@ using namespace ::mars::interfaces; class ODEObjectFactory : public ObjectFactoryInterface{ public: - static ODEObjectFactory* getInstance(); + static ODEObjectFactory& Instance(); + virtual std::shared_ptr createObject(std::shared_ptr worldPhysics, NodeData * nodeData) override; protected: ODEObjectFactory(); virtual ~ODEObjectFactory(); - virtual std::shared_ptr createObject(std::shared_ptr worldPhysics, NodeData * nodeData) override; - -private: - static ODEObjectFactory* instance; }; } diff --git a/sim/src/physics/ODEPlane.cpp b/sim/src/physics/ODEPlane.cpp deleted file mode 100644 index a0807b83c..000000000 --- a/sim/src/physics/ODEPlane.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/* - * 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" - -namespace mars { -namespace sim { - - using namespace utils; - using namespace interfaces; - - ODEPlane::ODEPlane(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODEPlane Constructor " << __FILE__ << ":" << __LINE__ << std::endl; - createNode(nodeData); // pass a function pointer? - } - - ODEPlane::~ODEPlane(void) { - std::cout << "DEBUGGG: in ODEPlane Destructor " << __FILE__ << ":" << __LINE__ << std::endl; - } - - bool ODEPlane::createODEGeometry(interfaces::NodeData *node){ - // build the ode representation - nGeom = dCreatePlane(theWorld->getSpace(), 0, 0, 1, (dReal)node->pos.z()); - std::cout << "Created Plane!" << std::endl; - return true; - } -} // end of namespace sim -} // end of namespace mars diff --git a/sim/src/physics/ODEPlane.h b/sim/src/physics/ODEPlane.h deleted file mode 100644 index cd3f15104..000000000 --- a/sim/src/physics/ODEPlane.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * 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 Muhammad Haider Khan Lodhi - * \brief "ODEPlane" implements an ODE plane geometry and collision using the ODEObject class - * - */ - -#ifndef ODE_PLANE_H -#define ODE_PLANE_H - -#ifdef _PRINT_HEADER_ - #warning "ODEPlane.h" -#endif - -#include -#include "ODEObject.h" - -//TODO remove? -#ifndef ODE11 - #define dTriIndex int -#endif - -namespace mars { - namespace sim { - - /** - * The class that implements the NodeInterface interface. - * - */ - class ODEPlane : public ODEObject { - public: - ODEPlane(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODEPlane(void); - 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/ODESphere.cpp b/sim/src/physics/ODESphere.cpp deleted file mode 100644 index dc345599f..000000000 --- a/sim/src/physics/ODESphere.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/* - * 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" - -namespace mars { - namespace sim { - - using namespace utils; - using namespace interfaces; - - ODESphere::ODESphere(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODESphere Constructor " << __FILE__ << ":" << __LINE__ << std::endl; - createNode(nodeData); - } - - ODESphere::~ODESphere(void) { - std::cout << "DEBUGGG: in ODESphere Destructor " << __FILE__ << ":" << __LINE__ << std::endl; - } - - /** - * 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()); - } - std::cout <<"Created ODESphere!" << std::endl; - return true; - } - - } // end of namespace sim -} // end of namespace mars diff --git a/sim/src/physics/ODESphere.h b/sim/src/physics/ODESphere.h deleted file mode 100644 index 0debfc0a3..000000000 --- a/sim/src/physics/ODESphere.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * 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 Muhammad Haider Khan Lodhi - * \brief "ODESphere" implements an ODE sphere represenation for the given node - * - */ - -#ifndef ODE_SPHERE_H -#define ODE_SPHERE_H - -#ifdef _PRINT_HEADER_ - #warning "ODESphere.h" -#endif - -#include -#include "ODEObject.h" - -//TODO remove? -#ifndef ODE11 - #define dTriIndex int -#endif - -namespace mars { - namespace sim { - - /** - * The class that implements the NodeInterface interface. - * - */ - class ODESphere : public ODEObject { - public: - ODESphere(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODESphere(void); - virtual bool createODEGeometry(interfaces::NodeData *node) override; - }; - - } // end of namespace sim -} // end of namespace mars - -#endif // ODE_SPHERE_H From 493bfd65532374c774f29eb61f4470ef7ec5fe94 Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Mon, 15 Aug 2022 14:42:53 +0200 Subject: [PATCH 10/21] Cleanup debug comments --- sim/src/physics/CoreODEObjects.cpp | 35 +++------------------------- sim/src/physics/ODEObject.cpp | 7 ++---- sim/src/physics/ODEObject.h | 4 ++-- sim/src/physics/ODEObjectFactory.cpp | 2 +- 4 files changed, 8 insertions(+), 40 deletions(-) diff --git a/sim/src/physics/CoreODEObjects.cpp b/sim/src/physics/CoreODEObjects.cpp index 72166056d..7238ee014 100644 --- a/sim/src/physics/CoreODEObjects.cpp +++ b/sim/src/physics/CoreODEObjects.cpp @@ -28,13 +28,10 @@ namespace sim { using namespace interfaces; ODEBox::ODEBox(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODEBox Constructor " << __FILE__ << ":" << __LINE__ << std::endl; createNode(nodeData); // pass a function pointer? } ODEBox::~ODEBox(void) { - std::cout << "DEBUGGG: in ODEBox Destructor " << __FILE__ << ":" << __LINE__ << std::endl; } bool ODEBox::createODEGeometry(interfaces::NodeData *node){ @@ -65,19 +62,14 @@ namespace sim { dMassSetBoxTotal(&nMass, tempMass, (dReal)(node->ext.x()), (dReal)(node->ext.y()),(dReal)(node->ext.z())); } - - std::cout << "Created Box!" << std::endl; return true; } ODECapsule::ODECapsule(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODECapsule Constructor " << __FILE__ << ":" << __LINE__ << std::endl; createNode(nodeData); // pass a function pointer? } ODECapsule::~ODECapsule(void) { - std::cout << "DEBUGGG: in ODECapsule Destructor " << __FILE__ << ":" << __LINE__ << std::endl; } bool ODECapsule::createODEGeometry(interfaces::NodeData *node){ @@ -106,18 +98,14 @@ namespace sim { dMassSetCapsuleTotal(&nMass, (dReal)node->mass, 3, (dReal)node->ext.x(), (dReal)node->ext.y()); } - std::cout << "Created ODECapsule" << std::endl; return true; } ODECylinder::ODECylinder(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODECylinder Constructor " << __FILE__ << ":" << __LINE__ << std::endl; - createNode(nodeData); // pass a function pointer? + createNode(nodeData); } ODECylinder::~ODECylinder(void) { - std::cout << "DEBUGGG: in ODECylinder Destructor " << __FILE__ << ":" << __LINE__ << std::endl; } bool ODECylinder::createODEGeometry(interfaces::NodeData *node){ @@ -146,19 +134,15 @@ namespace sim { dMassSetCylinderTotal(&nMass, (dReal)node->mass, 3, (dReal)node->ext.x(), (dReal)node->ext.y()); } - std::cout << "Created ODECylinder!" << std::endl; return true; } ODEHeightField::ODEHeightField(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODEHeightField Constructor " << __FILE__ << ":" << __LINE__ << std::endl; height_data = 0; - createNode(nodeData); // pass a function pointer? + createNode(nodeData); } ODEHeightField::~ODEHeightField(void) { - std::cout << "DEBUGGG: in ODEHeightField Destructor " << __FILE__ << ":" << __LINE__ << std::endl; if(height_data) free(height_data); if(terrain) free (terrain); } @@ -208,18 +192,14 @@ namespace sim { dRSetIdentity(R); dRFromAxisAndAngle(R, 1, 0, 0, M_PI/2); dGeomSetRotation(nGeom, R); - std::cout << "Created ODEHeightField!" << std::endl; return true; } ODEMesh::ODEMesh(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODEMesh Constructor " << __FILE__ << ":" << __LINE__ << std::endl; createNode(nodeData); } ODEMesh::~ODEMesh(void) { - std::cout << "DEBUGGG: in ODEMesh Destructor " << __FILE__ << ":" << __LINE__ << std::endl; if(myVertices) free(myVertices); if(myIndices) free(myIndices); if(myTriMeshData) dGeomTriMeshDataDestroy(myTriMeshData); @@ -312,35 +292,27 @@ namespace sim { dMassSetBoxTotal(&nMass, (dReal)node->mass, (dReal)node->ext.x(), (dReal)node->ext.y(),(dReal)node->ext.z()); } - std::cout << "Created ODEMesh" << std::endl; return true; } ODEPlane::ODEPlane(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODEPlane Constructor " << __FILE__ << ":" << __LINE__ << std::endl; - createNode(nodeData); // pass a function pointer? + createNode(nodeData); } ODEPlane::~ODEPlane(void) { - std::cout << "DEBUGGG: in ODEPlane Destructor " << __FILE__ << ":" << __LINE__ << std::endl; } bool ODEPlane::createODEGeometry(interfaces::NodeData *node){ // build the ode representation nGeom = dCreatePlane(theWorld->getSpace(), 0, 0, 1, (dReal)node->pos.z()); - std::cout << "Created Plane!" << std::endl; return true; } ODESphere::ODESphere(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODESphere Constructor " << __FILE__ << ":" << __LINE__ << std::endl; createNode(nodeData); } ODESphere::~ODESphere(void) { - std::cout << "DEBUGGG: in ODESphere Destructor " << __FILE__ << ":" << __LINE__ << std::endl; } /** @@ -369,7 +341,6 @@ namespace sim { else if(node->mass > 0) { dMassSetSphereTotal(&nMass, (dReal)node->mass, (dReal)node->ext.x()); } - std::cout <<"Created ODESphere!" << std::endl; return true; } diff --git a/sim/src/physics/ODEObject.cpp b/sim/src/physics/ODEObject.cpp index 2d0de6a2e..07545eb36 100644 --- a/sim/src/physics/ODEObject.cpp +++ b/sim/src/physics/ODEObject.cpp @@ -37,7 +37,7 @@ namespace mars { - namespace sim { +namespace sim { using namespace utils; using namespace interfaces; @@ -57,8 +57,6 @@ namespace mars { * - the body and geom should be initialized to 0 */ ODEObject::ODEObject(std::shared_ptr world, NodeData * nodeData) { - // At this moment we have not much things to do here. ^_^ - std::cout << "DEBUGGG: in ODEObject Constructor " << __FILE__ << ":" << __LINE__ << std::endl; theWorld = std::dynamic_pointer_cast(world); nBody = 0; nGeom = 0; @@ -80,7 +78,6 @@ namespace mars { * are the geom and the body realy all thing to take care of? */ ODEObject::~ODEObject(void) { - std::cout << "DEBUGGG: in ODEObject Destructor " << __FILE__ << ":" << __LINE__ << std::endl; std::vector::iterator iter; MutexLocker locker(&(theWorld->iMutex)); @@ -1497,5 +1494,5 @@ namespace mars { return 0.0; } - } // end of namespace sim +} // end of namespace sim } // end of namespace mars diff --git a/sim/src/physics/ODEObject.h b/sim/src/physics/ODEObject.h index 34f5cd695..bb5a6a9bf 100644 --- a/sim/src/physics/ODEObject.h +++ b/sim/src/physics/ODEObject.h @@ -42,7 +42,7 @@ //TODO move struct descriptions to seperate file! namespace mars { - namespace sim { +namespace sim { /* * we need a data structure to handle different collision parameter @@ -151,7 +151,7 @@ namespace mars { void setInertiaMass(interfaces::NodeData *node); }; - } // end of namespace sim +} // end of namespace sim } // end of namespace mars #endif // NODE_PHYSICS_H diff --git a/sim/src/physics/ODEObjectFactory.cpp b/sim/src/physics/ODEObjectFactory.cpp index 67a07a452..4034a09d0 100644 --- a/sim/src/physics/ODEObjectFactory.cpp +++ b/sim/src/physics/ODEObjectFactory.cpp @@ -66,7 +66,7 @@ std::shared_ptr ODEObjectFactory::createObject(std::shared_ptr(nullptr); break; } From 162b946ce643d69116474de0c1ce8c5e8786a8ae Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Mon, 15 Aug 2022 16:16:10 +0200 Subject: [PATCH 11/21] Cleanup of unnecessary comments & code --- sim/src/physics/ODEObject.cpp | 2 +- sim/src/physics/ODEObjectFactory.cpp | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/sim/src/physics/ODEObject.cpp b/sim/src/physics/ODEObject.cpp index 07545eb36..0e4d2857c 100644 --- a/sim/src/physics/ODEObject.cpp +++ b/sim/src/physics/ODEObject.cpp @@ -217,7 +217,7 @@ namespace sim { MutexLocker locker(&(theWorld->iMutex)); if(theWorld && theWorld->existsWorld()) { bool ret; - ret = createODEGeometry(node); //could this be a lamda or a function pointer? + ret = createODEGeometry(node); if(ret == 0) { // Error createing the physical Node return 0; diff --git a/sim/src/physics/ODEObjectFactory.cpp b/sim/src/physics/ODEObjectFactory.cpp index 4034a09d0..6a50839f8 100644 --- a/sim/src/physics/ODEObjectFactory.cpp +++ b/sim/src/physics/ODEObjectFactory.cpp @@ -28,8 +28,6 @@ namespace sim{ using namespace ::mars::interfaces; -//ODEObjectFactory* ODEObjectFactory::instance = NULL; - ODEObjectFactory& ODEObjectFactory::Instance(){ static ODEObjectFactory instance; return instance; @@ -66,7 +64,7 @@ std::shared_ptr ODEObjectFactory::createObject(std::shared_ptr(nullptr); break; } From 0dcdf71083c4beb299942331df8b9a5c801f36f9 Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Mon, 15 Aug 2022 16:17:15 +0200 Subject: [PATCH 12/21] Replace JointPhysics by subclasses and a factory --- sim/CMakeLists.txt | 13 +- sim/src/physics/CoreODEJoints.cpp | 305 ++++++ sim/src/physics/CoreODEJoints.h | 110 +++ sim/src/physics/JointFactoryInterface.h | 44 + sim/src/physics/ODEJoint.cpp | 1197 +++++++++++++++++++++++ sim/src/physics/ODEJoint.h | 112 +++ sim/src/physics/ODEJointFactory.cpp | 73 ++ sim/src/physics/ODEJointFactory.h | 50 + 8 files changed, 1901 insertions(+), 3 deletions(-) create mode 100644 sim/src/physics/CoreODEJoints.cpp create mode 100644 sim/src/physics/CoreODEJoints.h create mode 100644 sim/src/physics/JointFactoryInterface.h create mode 100644 sim/src/physics/ODEJoint.cpp create mode 100644 sim/src/physics/ODEJoint.h create mode 100644 sim/src/physics/ODEJointFactory.cpp create mode 100644 sim/src/physics/ODEJointFactory.h diff --git a/sim/CMakeLists.txt b/sim/CMakeLists.txt index 47124ef69..fb36c077f 100644 --- a/sim/CMakeLists.txt +++ b/sim/CMakeLists.txt @@ -86,13 +86,17 @@ set(SOURCES_H src/core/Simulator.h src/sensors/RotatingRaySensor.h - src/physics/JointPhysics.h + #src/physics/JointPhysics.h src/physics/ODEObject.h + src/physics/ODEJoint.h src/physics/WorldPhysics.h src/physics/ContactsPhysics.hpp src/physics/ODEObjectFactory.h src/physics/ObjectFactoryInterface.h src/physics/CoreODEObjects.h + src/physics/ODEJointFactory.h + src/physics/JointFactoryInterface.h + src/physics/CoreODEJoints.h src/sensors/CameraSensor.h src/sensors/IDListConfig.h @@ -141,12 +145,15 @@ set(TARGET_SRC src/sensors/MultiLevelLaserRangeFinder.cpp src/sensors/RotatingRaySensor.cpp - src/physics/JointPhysics.cpp + #src/physics/JointPhysics.cpp + src/physics/ODEJoint.cpp src/physics/ODEObject.cpp src/physics/WorldPhysics.cpp src/physics/ODEObjectFactory.cpp src/physics/CoreODEObjects.cpp - + src/physics/ODEJointFactory.cpp + src/physics/CoreODEJoints.cpp + src/sensors/CameraSensor.cpp src/sensors/Joint6DOFSensor.cpp src/sensors/JointArraySensor.cpp diff --git a/sim/src/physics/CoreODEJoints.cpp b/sim/src/physics/CoreODEJoints.cpp new file mode 100644 index 000000000..00be2eaf1 --- /dev/null +++ b/sim/src/physics/CoreODEJoints.cpp @@ -0,0 +1,305 @@ +/* + * 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 "CoreODEJoints.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) { + } + + bool ODEHingeJoint::createODEJoint(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; + } + + 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) { + } + + bool ODEHinge2Joint::createODEJoint(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; + } + + 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) { + } + + bool ODESliderJoint::createODEJoint(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; + } + + 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) { + } + + bool ODEBallJoint::createODEJoint(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; + } + + 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) { + } + + bool ODEUniversalJoint::createODEJoint(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; + } + + 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) { + } + + bool ODEFixedJoint::createODEJoint(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/CoreODEJoints.h b/sim/src/physics/CoreODEJoints.h new file mode 100644 index 000000000..156b088c2 --- /dev/null +++ b/sim/src/physics/CoreODEJoints.h @@ -0,0 +1,110 @@ +/* + * 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 CoreODEJoints.h + * \author Malte Roemmermann, Muhammad Haider Khan Lodhi + * \brief A collection of OBE joint classes + * + */ + +#ifndef CORE_ODE_OBJECTS_H +#define CORE_ODE_OBJECTS_H + +#ifdef _PRINT_HEADER_ + #warning "CoreODEJoints.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); + virtual bool createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2) override; + }; + + 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); + virtual bool createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2) override; + }; + + 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); + virtual bool createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2) override; + }; + + 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); + virtual bool createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2) override; + }; + + 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); + virtual bool createODEJoint(interfaces::NodeData *node) override; + }; + + 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); + virtual bool createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2) override; + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // CORE_ODE_JOINTS_H diff --git a/sim/src/physics/JointFactoryInterface.h b/sim/src/physics/JointFactoryInterface.h new file mode 100644 index 000000000..167b93b38 --- /dev/null +++ b/sim/src/physics/JointFactoryInterface.h @@ -0,0 +1,44 @@ +/* + * 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 + +namespace mars{ +namespace sim{ + +using namespace ::mars::interfaces; + +class JointFactoryInterface{ +public: + virtual std::shared_ptr createJoint(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/ODEJoint.cpp b/sim/src/physics/ODEJoint.cpp new file mode 100644 index 000000000..d139055c8 --- /dev/null +++ b/sim/src/physics/ODEJoint.cpp @@ -0,0 +1,1197 @@ +/* + * 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 ODEJoint.h + * \autor Larbi Abdenebaoui + * \brief "ODEJoint" declares the physics for the joint using ode + * + * core "SimJoint" and the physics "ODEJoint" + * SimJoint ------> JointInterface(i_Joint) ------------> ODEJoint + * + */ + +#include + +#include "ODEJoint.h" +#include "ODEObject.h" + +#include + +namespace mars { +namespace sim { + + using namespace utils; + using namespace interfaces; + + /** + * \brief the constructor of the Joint physics + * initialize the attributes of the object + */ + ODEJoint::ODEJoint(std::shared_ptr world){ + theWorld = std::static_pointer_cast(world); + jointId = ball_motor = 0; + jointCFM = 0.0; + cfm = cfm1 = cfm2 = erp1 = erp2 = 0; + lo1 = lo2 = hi1 = hi2 = 0; + damping = 0; + spring = 0; + body1 = 0; + body2 = 0; + } + + /** + * \brief Destroys the joint in the physics world. + * + * pre: + * - theWorld is the correct world object + * + * post: + * - all physical representation of the joint should be cleared + */ + ODEJoint::~ODEJoint(void) { + MutexLocker locker(&(theWorld->iMutex)); + if (jointId) { + dJointDestroy(jointId); + } + } + + + void ODEJoint::calculateCfmErp(const JointData *jointS) { + // how to set erp and cfm + // ERP = h kp / (h kp + kd) + // CFM = 1 / (h kp + kd) + damping = (dReal)jointS->damping_const_constraint_axis1; + spring = (dReal)jointS->spring_const_constraint_axis1; + dReal h = theWorld->getWorldStep(); + cfm = damping; + erp1 = h*(dReal)jointS->spring_const_constraint_axis1 + +(dReal)jointS->damping_const_constraint_axis1; + cfm1 = h*(dReal)jointS->spring_const_constraint_axis1 + +(dReal)jointS->damping_const_constraint_axis1; + erp2 = h*(dReal)jointS->spring_const_constraint_axis2 + +(dReal)jointS->damping_const_constraint_axis2; + cfm2 = h*(dReal)jointS->spring_const_constraint_axis2 + +(dReal)jointS->damping_const_constraint_axis2; + + lo1 = jointS->lowStopAxis1; + hi1 = jointS->highStopAxis1; + lo2 = jointS->lowStopAxis2; + hi2 = jointS->highStopAxis2; + // we don't want to run in the trap where kp and kd are set to zero + cfm = (cfm>0)?1/cfm:0.000000000001; + erp1 = (erp1>0)?h*(dReal)jointS->spring_const_constraint_axis1/erp1:0; + cfm1 = (cfm1>0)?1/cfm1:0.00000000001; + erp2 = (erp2>0)?h*(dReal)jointS->spring_const_constraint_axis2/erp2:0; + cfm2 = (cfm2>0)?1/cfm2:0.000000000001; + } + + /** + * \brief create the joint with the informations giving from jointS + * + */ + bool ODEJoint::createJoint(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", + jointS->index, jointS->nodeIndex1, jointS->nodeIndex2, + jointS->anchor.x(), jointS->anchor.y(), jointS->anchor.z(), + jointS->axis1.x(), jointS->axis1.y(), jointS->axis1.z()); +#endif + MutexLocker locker(&(theWorld->iMutex)); + 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); + dBodyID b1 = 0, b2 = 0; + + calculateCfmErp(jointS); + if(jointS->config.hasKey("jointCFM")) { + jointCFM = jointS->config["jointCFM"]; + } + + joint_type = jointS->type; + if(n1) b1 = n1->getBody(); + if(n2) b2 = n2->getBody(); + body1 = b1; + body2 = b2; + + bool ret; + ret = createODEJoint(jointS, body1, body2); + if(ret == 0) { + // Error createing the joint + return 0; + } + + // 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); + return 1; + } + return 0; + } + + bool ODEJoint::createODEJoint(interfaces::NodeData *node){ + 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 true; + } + + ///get the anchor of the joint + void ODEJoint::getAnchor(Vector* anchor) const { + dReal pos[4] = {0,0,0,0}; + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointGetHingeAnchor(jointId, pos); + break; + case JOINT_TYPE_HINGE2: + dJointGetHinge2Anchor(jointId, pos); + break; + case JOINT_TYPE_SLIDER: + // the slider joint has no ancher point + break; + case JOINT_TYPE_BALL: + dJointGetBallAnchor(jointId, pos); + break; + case JOINT_TYPE_UNIVERSAL: + dJointGetUniversalAnchor(jointId, pos); + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + anchor->x() = pos[0]; + anchor->y() = pos[1]; + anchor->z() = pos[2]; + } + + // the next force and velocity methods are only in a beta state + void ODEJoint::setForceLimit(sReal max_force) { + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointSetHingeParam(jointId, dParamFMax, (dReal)max_force); + break; + case JOINT_TYPE_HINGE2: + dJointSetHinge2Param(jointId, dParamFMax, (dReal)max_force); + break; + case JOINT_TYPE_SLIDER: + dJointSetSliderParam(jointId, dParamFMax, (dReal)max_force); + break; + case JOINT_TYPE_UNIVERSAL: + dJointSetUniversalParam(jointId, dParamFMax, (dReal)max_force); + break; + } + } + + void ODEJoint::setForceLimit2(sReal max_force) { + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_HINGE: + break; + case JOINT_TYPE_HINGE2: + dJointSetHinge2Param(jointId, dParamFMax2, (dReal)max_force); + break; + case JOINT_TYPE_SLIDER: + break; + case JOINT_TYPE_UNIVERSAL: + dJointSetUniversalParam(jointId, dParamFMax2, (dReal)max_force); + break; + } + } + + void ODEJoint::setVelocity(sReal velocity) { + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointSetHingeParam(jointId, dParamVel, (dReal)velocity); + break; + case JOINT_TYPE_HINGE2: + dJointSetHinge2Param(jointId, dParamVel, (dReal)velocity); + break; + case JOINT_TYPE_SLIDER: + dJointSetSliderParam(jointId, dParamVel, (dReal)velocity); + break; + case JOINT_TYPE_UNIVERSAL: + dJointSetUniversalParam(jointId, dParamVel, (dReal)velocity); + break; + } + } + + void ODEJoint::setVelocity2(sReal velocity) { + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_HINGE: + break; + case JOINT_TYPE_HINGE2: + dJointSetHinge2Param(jointId, dParamVel2, (dReal)velocity); + break; + case JOINT_TYPE_SLIDER: + break; + case JOINT_TYPE_UNIVERSAL: + dJointSetUniversalParam(jointId, dParamVel2, (dReal)velocity); + break; + } + } + + sReal ODEJoint::getPosition(void) const { + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_HINGE: + return (sReal)dJointGetHingeAngle(jointId); + break; + case JOINT_TYPE_HINGE2: + return (sReal)dJointGetHinge2Angle1(jointId); + break; + case JOINT_TYPE_SLIDER: + return (sReal)dJointGetSliderPosition(jointId); + break; + case JOINT_TYPE_UNIVERSAL: + return (sReal)dJointGetUniversalAngle1(jointId); + break; + } + return 0; + } + + sReal ODEJoint::getPosition2(void) const { + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_UNIVERSAL: + return (sReal)dJointGetUniversalAngle2(jointId); + break; + } + return 0; + } + + /// set the anchor i.e. the position where the joint is created of the joint + void ODEJoint::setAnchor(const Vector &anchor){ + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointSetHingeAnchor(jointId, anchor.x(), anchor.y(), anchor.z()); + //std::cout << " " << anchor.z(); + break; + case JOINT_TYPE_HINGE2: + dJointSetHinge2Anchor(jointId, anchor.x(), anchor.y(), anchor.z()); + break; + case JOINT_TYPE_SLIDER: + // the slider joint has no ancher point + break; + case JOINT_TYPE_BALL: + dJointSetBallAnchor(jointId, anchor.x(), anchor.y(), anchor.z()); + break; + case JOINT_TYPE_UNIVERSAL: + dJointSetUniversalAnchor(jointId, anchor.x(), anchor.y(), anchor.z()); + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + } + + /** + * \brief Set the Axis of the Joint to a new position + * + * pre: + * + * post: + */ + void ODEJoint::setAxis(const Vector &axis){ + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointSetHingeAxis(jointId, axis.x(), axis.y(), axis.z()); + break; + case JOINT_TYPE_HINGE2: + dJointSetHinge2Axis1(jointId, axis.x(), axis.y(), axis.z()); + break; + case JOINT_TYPE_SLIDER: + dJointSetSliderAxis(jointId, axis.x(), axis.y(), axis.z()); + break; + case JOINT_TYPE_BALL: + // the ball joint has no axis + break; + case JOINT_TYPE_UNIVERSAL: + dJointSetUniversalAxis1(jointId, axis.x(), axis.y(), axis.z()); + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + } + + /** + * \brief Set the Axis2 of the Joint + * + * pre: + * + * post: + */ + void ODEJoint::setAxis2(const Vector &axis){ + MutexLocker locker(&(theWorld->iMutex)); + switch(joint_type) { + case JOINT_TYPE_HINGE: + // the hinge joint has only one axis + break; + case JOINT_TYPE_HINGE2: + dJointSetHinge2Axis2(jointId, axis.x(), axis.y(), axis.z()); + break; + case JOINT_TYPE_SLIDER: + // the slider joint has only one axis + break; + case JOINT_TYPE_BALL: + // the ball joint has no axis + break; + case JOINT_TYPE_UNIVERSAL: + dJointSetUniversalAxis2(jointId, axis.x(), axis.y(), axis.z()); + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + } + + /** + * \brief Gets the actual axis of a joint + * + * pre: + * - the joint should be created and should have a axis vector + * + * post: + * - the given axis struct should be filled with correct values + */ + void ODEJoint::getAxis(Vector* axis) const { + dReal pos[4] = {0,0,0,0}; + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointGetHingeAxis(jointId, pos); + break; + case JOINT_TYPE_HINGE2: + dJointGetHinge2Axis1(jointId, pos); + break; + case JOINT_TYPE_SLIDER: + dJointGetSliderAxis(jointId, pos); + break; + case JOINT_TYPE_BALL: + // the ball joint has no axis + break; + case JOINT_TYPE_UNIVERSAL: + dJointGetUniversalAxis1(jointId, pos); + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + axis->x() = (sReal)pos[0]; + axis->y() = (sReal)pos[1]; + axis->z() = (sReal)pos[2]; + } + + /** + * \brief Gets the actual second axis of a joint + * + * pre: + * - the joint should be created and should have second axis vector + * + * post: + * - the given axis struct should be filled with correct values + */ + void ODEJoint::getAxis2(Vector* axis) const { + dReal pos[4] = {0,0,0,0}; + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_HINGE: + // the hinge joint has only one axis + break; + case JOINT_TYPE_HINGE2: + dJointGetHinge2Axis2(jointId, pos); + break; + case JOINT_TYPE_SLIDER: + // the slider joint has only one axis + break; + case JOINT_TYPE_BALL: + // the ball joint has no axis + break; + case JOINT_TYPE_UNIVERSAL: + dJointGetUniversalAxis2(jointId, pos); + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + axis->x() = (sReal)pos[0]; + axis->y() = (sReal)pos[1]; + axis->z() = (sReal)pos[2]; + } + + ///set the world informations + void ODEJoint::setWorldObject(std::shared_ptr world){ + theWorld = std::static_pointer_cast(world); + } + + void ODEJoint::setJointAsMotor(int axis) { + MutexLocker locker(&(theWorld->iMutex)); + switch(joint_type) { + // todo: need to handle the distinction whether to set or not to + // set the hi and low stop differently + case JOINT_TYPE_HINGE: + //if(!lo1 && !hi) + { + dJointSetHingeParam(jointId, dParamLoStop, -dInfinity); + dJointSetHingeParam(jointId, dParamHiStop, dInfinity); + } + break; + case JOINT_TYPE_HINGE2: + if(!lo1 && !hi1 && axis == 1) { + dJointSetHinge2Param(jointId, dParamLoStop, -dInfinity); + dJointSetHinge2Param(jointId, dParamHiStop, dInfinity); + } + if(!lo2 && !hi2 && axis == 2) { + dJointSetHinge2Param(jointId, dParamLoStop2, -dInfinity); + dJointSetHinge2Param(jointId, dParamHiStop2, dInfinity); + } + break; + case JOINT_TYPE_SLIDER: + if(!lo1 && !hi1) { + dJointSetSliderParam(jointId, dParamLoStop, -dInfinity); + dJointSetSliderParam(jointId, dParamHiStop, dInfinity); + } + break; + case JOINT_TYPE_UNIVERSAL: + /* + if(!lo1 && !hi1 && axis == 1) { + dJointSetUniversalParam(jointId, dParamLoStop, -dInfinity); + dJointSetUniversalParam(jointId, dParamHiStop, dInfinity); + } + if(!lo2 && !hi2 && axis == 2) { + dJointSetUniversalParam(jointId, dParamLoStop2, -dInfinity); + dJointSetUniversalParam(jointId, dParamHiStop2, dInfinity); + } + */ + break; + } + } + + void ODEJoint::unsetJointAsMotor(int axis) { + MutexLocker locker(&(theWorld->iMutex)); + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointSetHingeParam(jointId, dParamLoStop, lo1); + dJointSetHingeParam(jointId, dParamHiStop, hi1); + break; + case JOINT_TYPE_HINGE2: + if(axis == 1) { + dJointSetHinge2Param(jointId, dParamLoStop, lo1); + dJointSetHinge2Param(jointId, dParamHiStop, hi1); + } + else if(axis == 2) { + dJointSetHinge2Param(jointId, dParamLoStop2, lo2); + dJointSetHinge2Param(jointId, dParamHiStop2, hi2); + } + break; + case JOINT_TYPE_SLIDER: + dJointSetSliderParam(jointId, dParamLoStop, lo1); + dJointSetSliderParam(jointId, dParamHiStop, hi1); + break; + case JOINT_TYPE_UNIVERSAL: + if(axis == 1) { + dJointSetUniversalParam(jointId, dParamLoStop, lo1); + dJointSetUniversalParam(jointId, dParamHiStop, hi1); + } + else if(axis == 2) { + dJointSetUniversalParam(jointId, dParamLoStop2, lo2); + dJointSetUniversalParam(jointId, dParamHiStop2, hi2); + } + break; + } + } + + /** + * \brief Gets the force the joint applies to the first body + * + * pre: + * - the joint should be created + * + * post: + * + */ + void ODEJoint::getForce1(Vector *f) const { + f->x() = (sReal)feedback.f1[0]; + f->y() = (sReal)feedback.f1[1]; + f->z() = (sReal)feedback.f1[2]; + } + + /** + * \brief Gets the force the joint applies to the second body + * + * pre: + * - the joint should be created + * + * post: + * + */ + void ODEJoint::getForce2(Vector *f) const { + f->x() = (sReal)feedback.f2[0]; + f->y() = (sReal)feedback.f2[1]; + f->z() = (sReal)feedback.f2[2]; + } + + /** + * \brief Gets the torque the joint applies to the first body + * + * pre: + * - the joint should be created + * + * post: + * + */ + void ODEJoint::getTorque1(Vector *t) const { + t->x() = (sReal)feedback.t1[0]; + t->y() = (sReal)feedback.t1[1]; + t->z() = (sReal)feedback.t1[2]; + } + + /** + * \brief Gets the torque the joint applies to the second body + * + * pre: + * - the joint should be created + * + * post: + * + */ + void ODEJoint::getTorque2(Vector *t) const { + t->x() = (sReal)feedback.t2[0]; + t->y() = (sReal)feedback.t2[1]; + t->z() = (sReal)feedback.t2[2]; + } + + /** + * \brief reset the achor to the actual position. If a Node is moved or + * rotated by the editor, this function resets the constrains the joint + * applies to the Nodes that are connected. + * + * pre: + * - the joint should be created + * + * post: + * + */ + void ODEJoint::reattacheJoint(void) { + dReal pos[4] = {0,0,0,0}; + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointGetHingeAnchor(jointId, pos); + dJointSetHingeAnchor(jointId, pos[0], pos[1], pos[2]); + break; + case JOINT_TYPE_HINGE2: + dJointGetHinge2Anchor(jointId, pos); + dJointSetHinge2Anchor(jointId, pos[0], pos[1], pos[2]); + break; + case JOINT_TYPE_SLIDER: + // the slider joint has no ancher point + break; + case JOINT_TYPE_BALL: + dJointGetBallAnchor(jointId, pos); + dJointSetBallAnchor(jointId, pos[0], pos[1], pos[2]); + break; + case JOINT_TYPE_UNIVERSAL: + dJointGetUniversalAnchor(jointId, pos); + dJointSetUniversalAnchor(jointId, pos[0], pos[1], pos[2]); + break; + case JOINT_TYPE_FIXED: + dJointDestroy(jointId); + jointId = dJointCreateFixed(theWorld->getWorld(), 0); + dJointAttach(jointId, body1, body2); + dJointSetFixed(jointId); + dJointSetFeedback(jointId, &feedback); + // used for the integration study of the SpaceClimber + //dJointSetFixedParam(jointId, dParamCFM, cfm1);//0.0002); + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + } + + /** + * \brief Return the torque vector for the first axis of the joint + * + * v1[0] = b1_pos[0] - anchor[0]; + * v1[1] = b1_pos[1] - anchor[1]; + * v1[2] = b1_pos[2] - anchor[2]; + * radius = dSqrt(v1[0]*v1[0]+ + * v1[1]*v1[1]+ + * v1[2]*v1[2]); + * normal[0] = axis[1]*v1[2] - axis[2]*v1[1]; + * normal[1] = -axis[0]*v1[2] + axis[2]*v1[0]; + * normal[2] = axis[0]*v1[1] - axis[1]*v1[0]; + * dot = (normal[0]*feedback.f1[0]+ + * normal[1]*feedback.f1[1]+ + * normal[2]*feedback.f1[2]); + * normal[0] *= dot*radius; + * normal[1] *= dot*radius; + * normal[2] *= dot*radius; + */ + void ODEJoint::getAxisTorque(Vector *t) const { + t->x() = axis1_torque.x(); + t->y() = axis1_torque.y(); + t->z() = axis1_torque.z(); + } + + void ODEJoint::getAxis2Torque(Vector *t) const { + t->x() = axis2_torque.x(); + t->y() = axis2_torque.y(); + t->z() = axis2_torque.z(); + } + + /** + * \brief we need to calculate the axis torques and joint load before + * we can return it. + * + */ + void ODEJoint::update(void) { + const dReal *b1_pos, *b2_pos; + dReal anchor[4], axis[4], axis2[4]; + int calc1 = 0, calc2 = 0; + dReal radius, dot, torque; + dReal v1[3], normal[3], load[3], tmp1[3], axis_force[3]; + MutexLocker locker(&(theWorld->iMutex)); + + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointGetHingeAnchor(jointId, anchor); + dJointGetHingeAxis(jointId, axis); + calc1 = 1; + break; + case JOINT_TYPE_HINGE2: + dJointGetHinge2Anchor(jointId, anchor); + dJointGetHinge2Axis1(jointId, axis); + dJointGetHinge2Axis2(jointId, axis2); + calc1 = 1; + calc2 = 0; + break; + case JOINT_TYPE_SLIDER: + dJointGetSliderAxis(jointId, axis); + calc1 = 2; + break; + case JOINT_TYPE_BALL: + // no axis + break; + case JOINT_TYPE_UNIVERSAL: + dJointGetUniversalAnchor(jointId, anchor); + dJointGetUniversalAxis1(jointId, axis); + dJointGetUniversalAxis2(jointId, axis2); + calc1 = 1; + calc2 = 0; + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + motor_torque = feedback.lambda; + axis1_torque.x() = axis1_torque.y() = axis1_torque.z() = 0; + axis2_torque.x() = axis2_torque.y() = axis2_torque.z() = 0; + joint_load.x() = joint_load.y() = joint_load.z() = 0; + if(calc1 == 1) { + if(body1) { + b1_pos = dBodyGetPosition(body1); + dOP(v1, -, b1_pos, anchor); + //radius = dLENGTH(v1); + dCROSS(normal, =, axis, v1); + dot = dDOT(normal, feedback.f1); + dOPEC(normal, *=, dot); + dOP(load, -, feedback.f1, normal); + dCROSS(tmp1, =, v1, normal); + axis1_torque.x() = (sReal)tmp1[0]; + axis1_torque.y() = (sReal)tmp1[1]; + axis1_torque.z() = (sReal)tmp1[2]; + dCROSS(tmp1, =, v1, load); + joint_load.x() = (sReal)tmp1[0]; + joint_load.y() = (sReal)tmp1[1]; + joint_load.z() = (sReal)tmp1[2]; + // now nearly the same for the torque + dot = dDOT(axis, feedback.t1); + dOPC(tmp1, *, axis, dot); + dOP(load, -, feedback.t1, tmp1); + axis1_torque.x() += (sReal)tmp1[0]; + axis1_torque.y() += (sReal)tmp1[1]; + axis1_torque.z() += (sReal)tmp1[2]; + joint_load.x() += (sReal)load[0]; + joint_load.y() += (sReal)load[1]; + joint_load.z() += (sReal)load[2]; + } + else if(body2) { + // now we do it correct + // first get the position vector v1 + b2_pos = dBodyGetPosition(body2); + dOP(v1, -, b2_pos, anchor); + + // then differentiate the torque from the force + dot = dDOT(feedback.f2, v1) / dDOT(v1, v1); + dOPC(axis_force, *, v1, dot); + + // the difference is the torque + dOP(tmp1, -, feedback.f2, axis_force); + + // the torque value is given by: + torque = dLENGTH(tmp1) / dLENGTH(v1); + // then get the normal to v1 and the torque vector + dCROSS(normal, =, v1, tmp1); + // and scale the normal to represent the correct torque length + dOPEC(normal, *=, torque / dLENGTH(normal)); + // now the normal represents the torque vector at the anchor + // and we make a projection to the axis + dot = dDOT(normal, axis); + dOPC(tmp1, *, axis, dot); + dOP(load, -, normal, tmp1); + //dCROSS(tmp1, =, v1, normal); + axis1_torque.x() = (sReal)tmp1[0]; + axis1_torque.y() = (sReal)tmp1[1]; + axis1_torque.z() = (sReal)tmp1[2]; + //dCROSS(tmp1, =, v1, load); + joint_load.x() = (sReal)load[0]; + joint_load.y() = (sReal)load[1]; + joint_load.z() = (sReal)load[2]; + // now nearly the same for the torque + dot = dDOT(feedback.t2, axis); + dOPC(tmp1, *, axis, dot); + dOP(load, -, feedback.t2, tmp1); + //axis1_torque.x() += (sReal)tmp1[0]; + //axis1_torque.y() += (sReal)tmp1[1]; + //axis1_torque.z() += (sReal)tmp1[2]; + joint_load.x() += (sReal)load[0]; + joint_load.y() += (sReal)load[1]; + joint_load.z() += (sReal)load[2]; + } + } + else if(calc1 == 2) { + // this is for the slider + } + if(calc2 == 1) { + if(body1) { + b1_pos = dBodyGetPosition(body1); + dOP(v1, -, b1_pos, anchor); + radius = dLENGTH(v1); + dCROSS(normal, =, axis2, v1); + dot = dDOT(normal, feedback.f1); + dOPEC(normal, *=, dot*radius); + axis2_torque.x() = (sReal)normal[0]; + axis2_torque.y() = (sReal)normal[1]; + axis2_torque.z() = (sReal)normal[2]; + // now nearly the same for the torque + dot = dDOT(axis2, feedback.t1); + axis2_torque.x() += (sReal)(axis2[0]*dot); + axis2_torque.y() += (sReal)(axis2[1]*dot); + axis2_torque.z() += (sReal)(axis2[2]*dot); + } + if(body2) { + b2_pos = dBodyGetPosition(body2); + dOP(v1, -, b2_pos, anchor); + radius = dLENGTH(v1); + dCROSS(normal, =, axis2, v1); + dot = dDOT(normal, feedback.f2); + dOPEC(normal, *=, dot*radius); + axis2_torque.x() += (sReal)normal[0]; + axis2_torque.y() += (sReal)normal[1]; + axis2_torque.z() += (sReal)normal[2]; + // now nearly the same for the torque + dot = dDOT(axis2, feedback.t2); + axis2_torque.x() += (sReal)(axis2[0]*dot); + axis2_torque.y() += (sReal)(axis2[1]*dot); + axis2_torque.z() += (sReal)(axis2[2]*dot); + } + } + } + + void ODEJoint::getJointLoad(Vector *t) const { + t->x() = joint_load.x(); + t->y() = joint_load.y(); + t->z() = joint_load.z(); + } + + sReal ODEJoint::getVelocity(void) const { + MutexLocker locker(&(theWorld->iMutex)); + switch(joint_type) { + case JOINT_TYPE_HINGE: + return (sReal)dJointGetHingeAngleRate(jointId); + break; + case JOINT_TYPE_HINGE2: + return (sReal)dJointGetHinge2Angle1Rate(jointId); + break; + case JOINT_TYPE_SLIDER: + return (sReal)dJointGetSliderPositionRate(jointId); + break; + case JOINT_TYPE_BALL: + // no axis + break; + case JOINT_TYPE_UNIVERSAL: + return (sReal)dJointGetUniversalAngle1Rate(jointId); + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + return 0; + } + + sReal ODEJoint::getVelocity2(void) const { + MutexLocker locker(&(theWorld->iMutex)); + switch(joint_type) { + case JOINT_TYPE_HINGE: + break; + case JOINT_TYPE_HINGE2: + return (sReal)dJointGetHinge2Angle2Rate(jointId); + break; + case JOINT_TYPE_SLIDER: + break; + case JOINT_TYPE_BALL: + // no axis + break; + case JOINT_TYPE_UNIVERSAL: + return (sReal)dJointGetUniversalAngle2Rate(jointId); + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + return 0; + } + + void ODEJoint::setTorque(sReal torque) { + MutexLocker locker(&(theWorld->iMutex)); + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointAddHingeTorque(jointId, torque); + break; + case JOINT_TYPE_HINGE2: + break; + case JOINT_TYPE_SLIDER: + dJointAddSliderForce(jointId, torque); + break; + case JOINT_TYPE_BALL: + // no axis + break; + case JOINT_TYPE_UNIVERSAL: + + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + } + + void ODEJoint::setTorque2(sReal torque) { + CPP_UNUSED(torque); + switch(joint_type) { + case JOINT_TYPE_HINGE: + break; + case JOINT_TYPE_HINGE2: + break; + case JOINT_TYPE_SLIDER: + break; + case JOINT_TYPE_BALL: + // no axis + break; + case JOINT_TYPE_UNIVERSAL: + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + } + + void ODEJoint::changeStepSize(const JointData &jointS) { + MutexLocker locker(&(theWorld->iMutex)); + if(theWorld && theWorld->existsWorld()) { + calculateCfmErp(&jointS); + + switch(jointS.type) { + case JOINT_TYPE_HINGE: + 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); + } + else if(lo1 != 0) { + dJointSetHingeParam(jointId, dParamLoStop, lo1); + dJointSetHingeParam(jointId, dParamHiStop, hi1); + } + break; + case JOINT_TYPE_HINGE2: + 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); + } + break; + case JOINT_TYPE_SLIDER: + if(damping > 0.00000001) { + dJointSetSliderParam(jointId, dParamFMax, damping); + dJointSetSliderParam(jointId, dParamVel, 0); + } + if(spring > 0.00000001) { + dJointSetSliderParam(jointId, dParamLoStop, lo1); + dJointSetSliderParam(jointId, dParamHiStop, hi1); + dJointSetSliderParam(jointId, dParamStopCFM, cfm1); + dJointSetSliderParam(jointId, dParamStopERP, erp1); + } + else if(lo1 != 0) { + dJointSetSliderParam(jointId, dParamLoStop, lo1); + dJointSetSliderParam(jointId, dParamHiStop, hi1); + } + break; + case JOINT_TYPE_BALL: + /* + 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); + } + */ + break; + case JOINT_TYPE_UNIVERSAL: + 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); + } + break; + default: + // no correct type is spezified, so no physically node will be created + break; + } + } + } + + sReal ODEJoint::getMotorTorque(void) const { + return motor_torque; + } + + interfaces::sReal ODEJoint::getLowStop() const { + switch(joint_type) { + case JOINT_TYPE_HINGE: + return dJointGetHingeParam(jointId, dParamLoStop); + case JOINT_TYPE_HINGE2: + return dJointGetHinge2Param(jointId, dParamLoStop); + case JOINT_TYPE_SLIDER: + return dJointGetSliderParam(jointId, dParamLoStop); + default: + // not implemented yes + fprintf(stderr, "mars::sim::ODEJoint: lowstop for type %d not implemented yet\n", joint_type); + return 0.; + } + } + + interfaces::sReal ODEJoint::getHighStop() const { + switch(joint_type) { + case JOINT_TYPE_HINGE: + return dJointGetHingeParam(jointId, dParamHiStop); + case JOINT_TYPE_HINGE2: + return dJointGetHinge2Param(jointId, dParamHiStop); + case JOINT_TYPE_SLIDER: + return dJointGetSliderParam(jointId, dParamHiStop); + default: + // not implemented yes + fprintf(stderr, "mars::sim::ODEJoint: highstop for type %d not implemented yet\n", joint_type); + return 0.; + } + } + + interfaces::sReal ODEJoint::getLowStop2() const { + switch(joint_type) { + case JOINT_TYPE_HINGE2: + return dJointGetHinge2Param(jointId, dParamLoStop2); + default: + // not implemented yes + fprintf(stderr, "mars::sim::ODEJoint: lowstop for type %d not implemented yet\n", joint_type); + return 0.; + } + } + + interfaces::sReal ODEJoint::getHighStop2() const { + switch(joint_type) { + case JOINT_TYPE_HINGE2: + return dJointGetHinge2Param(jointId, dParamHiStop2); + default: + // not implemented yes + fprintf(stderr, "mars::sim::ODEJoint: highstop for type %d not implemented yet\n", joint_type); + return 0.; + } + } + + interfaces::sReal ODEJoint::getCFM() const { + switch(joint_type) { + case JOINT_TYPE_HINGE: + return dJointGetHingeParam(jointId, dParamCFM); + case JOINT_TYPE_HINGE2: + return dJointGetHinge2Param(jointId, dParamCFM); + case JOINT_TYPE_SLIDER: + return dJointGetSliderParam(jointId, dParamCFM); + default: + // not implemented yes + fprintf(stderr, "mars::sim::ODEJoint: cfm for type %d not implemented yet\n", joint_type); + return 0.; + } + } + + void ODEJoint::setLowStop(interfaces::sReal lowStop) { + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointSetHingeParam(jointId, dParamLoStop, lowStop); + break; + case JOINT_TYPE_HINGE2: + dJointSetHinge2Param(jointId, dParamLoStop, lowStop); + break; + case JOINT_TYPE_SLIDER: + dJointSetSliderParam(jointId, dParamLoStop, lowStop); + break; + default: + // not implemented yes + fprintf(stderr, "mars::sim::ODEJoint: highstop for type %d not implemented yet\n", joint_type); + break; + } + } + + void ODEJoint::setHighStop(interfaces::sReal highStop) { + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointSetHingeParam(jointId, dParamHiStop, highStop); + break; + case JOINT_TYPE_HINGE2: + dJointSetHinge2Param(jointId, dParamHiStop, highStop); + break; + case JOINT_TYPE_SLIDER: + dJointSetSliderParam(jointId, dParamHiStop, highStop); + break; + default: + // not implemented yes + fprintf(stderr, "mars::sim::ODEJoint: highstop for type %d not implemented yet\n", joint_type); + break; + } + } + + 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::ODEJoint: highstop for type %d not implemented yet\n", joint_type); + break; + } + } + + 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::ODEJoint: highstop for type %d not implemented yet\n", joint_type); + break; + } + } + + void ODEJoint::setCFM(interfaces::sReal cfm) { + switch(joint_type) { + case JOINT_TYPE_HINGE: + dJointSetHingeParam(jointId, dParamCFM, cfm); + break; + case JOINT_TYPE_HINGE2: + dJointSetHinge2Param(jointId, dParamCFM, cfm); + break; + case JOINT_TYPE_SLIDER: + dJointSetSliderParam(jointId, dParamCFM, cfm); + break; + default: + // not implemented yes + fprintf(stderr, "mars::sim::ODEJoint: cfm for type %d not implemented yet\n", joint_type); + break; + } + } + +} // end of namespace sim +} // end of namespace mars diff --git a/sim/src/physics/ODEJoint.h b/sim/src/physics/ODEJoint.h new file mode 100644 index 000000000..6a26bf073 --- /dev/null +++ b/sim/src/physics/ODEJoint.h @@ -0,0 +1,112 @@ +/* + * 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 . + * + */ + +#ifndef ODE_JOINT_H +#define ODE_JOINT_H + +#ifdef _PRINT_HEADER_ + #warning "ODEJoint.h" +#endif + +#include "WorldPhysics.h" + +#include +#include + +namespace mars { +namespace sim { + + class ODEJoint: public interfaces::JointInterface { + public: + ///the constructor + ODEJoint(std::shared_ptr world); + ///the destructor + 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(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 + virtual void setAnchor(const utils::Vector &anchor); + ///set the world informations + virtual void setAxis(const utils::Vector &axis); + virtual void setAxis2(const utils::Vector &axis); + virtual void getAxis(utils::Vector* axis) const; + virtual void getAxis2(utils::Vector* axis) const; + virtual void setWorldObject(std::shared_ptr world); + // methods to controll a joint through a motor + virtual void setForceLimit(interfaces::sReal max_force); + virtual void setForceLimit2(interfaces::sReal max_force); + virtual void setVelocity(interfaces::sReal velocity); + virtual void setVelocity2(interfaces::sReal velocity); + virtual interfaces::sReal getVelocity(void) const; + virtual interfaces::sReal getVelocity2(void) const; + virtual void setJointAsMotor(int axis); + virtual void unsetJointAsMotor(int axis); + virtual interfaces::sReal getPosition(void) const; + virtual interfaces::sReal getPosition2(void) const; + virtual void getForce1(utils::Vector *f) const; + virtual void getForce2(utils::Vector *f) const; + virtual void getTorque1(utils::Vector *t) const; + virtual void getTorque2(utils::Vector *t) const; + virtual void setTorque(interfaces::sReal torque); + virtual void setTorque2(interfaces::sReal torque); + virtual void reattacheJoint(void); + virtual void update(void); + virtual void getJointLoad(utils::Vector *t) const; + virtual void getAxisTorque(utils::Vector *t) const; + virtual void getAxis2Torque(utils::Vector *t) const; + virtual void changeStepSize(const interfaces::JointData &jointS); + virtual interfaces::sReal getMotorTorque(void) const; + virtual interfaces::sReal getLowStop() const; + virtual interfaces::sReal getHighStop() const; + virtual interfaces::sReal getLowStop2() const; + virtual interfaces::sReal getHighStop2() const; + virtual interfaces::sReal getCFM() const; + virtual void setLowStop(interfaces::sReal lowStop); + virtual void setHighStop(interfaces::sReal highStop); + virtual void setLowStop2(interfaces::sReal lowStop2); + virtual void setHighStop2(interfaces::sReal highStop2); + virtual void setCFM(interfaces::sReal cfm); + + protected: + std::shared_ptr theWorld; + dJointID jointId, ball_motor; + dJointFeedback feedback; + dBodyID body1, body2; + int joint_type; + dReal cfm, cfm1, cfm2, erp1, erp2; + dReal lo1, lo2, hi1, hi2; + dReal damping, spring, jointCFM; + utils::Vector axis1_torque, axis2_torque, joint_load; + dReal motor_torque; + + void calculateCfmErp(const interfaces::JointData *jointS); + }; + +} // end of namespace sim +} // end of namespace mars + +#endif // ODE_JOINT_H diff --git a/sim/src/physics/ODEJointFactory.cpp b/sim/src/physics/ODEJointFactory.cpp new file mode 100644 index 000000000..1d3e9b010 --- /dev/null +++ b/sim/src/physics/ODEJointFactory.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 +#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){ + switch(jointData->type) { + case JOINT_TYPE_HINGE: + return std::make_shared(worldPhysics, jointData, node1, node2); + break; + case JOINT_TYPE_HINGE2: + return std::make_shared(worldPhysics, jointData, node1, node2); + break; + case JOINT_TYPE_SLIDER: + return std::make_shared(worldPhysics, jointData, node1, node2); + break; + case JOINT_TYPE_BALL: + return std::make_shared(worldPhysics, jointData, node1, node2); + break; + case JOINT_TYPE_UNIVERSAL: + return std::make_shared(worldPhysics, jointData, node1, node2); + break; + case JOINT_TYPE_FIXED: + return std::make_shared(worldPhysics, jointData, node1, node2); + break; + default: + // no correct type is spezified, so no physically node will be created + std::cout << "Unknown type of ODEJoint requested. No physical joint was created." << std::endl; + return std::shared_ptr(nullptr); + break; + } +} +} +} diff --git a/sim/src/physics/ODEJointFactory.h b/sim/src/physics/ODEJointFactory.h new file mode 100644 index 000000000..ed5ec5bac --- /dev/null +++ b/sim/src/physics/ODEJointFactory.h @@ -0,0 +1,50 @@ +/* + * 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 + +namespace mars{ +namespace sim{ + +using namespace ::mars::interfaces; + +class ODEJointFactory : public JointFactoryInterface{ +public: + static ODEJointFactory& Instance(); + virtual std::shared_ptr createJoint(interfaces::JointData *joint, + const std::shared_ptr node1, + const std::shared_ptr node2) override; + +protected: + ODEJointFactory(); + virtual ~ODEJointFactory(); +}; + +} +} From 85fdcf7836ab5c6cec9957724037963950d9957e Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Mon, 15 Aug 2022 17:14:37 +0200 Subject: [PATCH 13/21] Add parameter to check if the joint was successfully created and cleanup function interfaces --- sim/src/core/JointManager.cpp | 34 ++++++++++++++----------- sim/src/physics/CoreODEJoints.cpp | 14 +++++----- sim/src/physics/CoreODEJoints.h | 12 ++++----- sim/src/physics/CoreODEObjects.cpp | 4 +-- sim/src/physics/JointFactoryInterface.h | 7 ++--- sim/src/physics/ODEJoint.cpp | 16 ++++++++---- sim/src/physics/ODEJoint.h | 7 ++--- sim/src/physics/ODEJointFactory.cpp | 23 ++++++++++++----- sim/src/physics/ODEJointFactory.h | 3 ++- 9 files changed, 72 insertions(+), 48 deletions(-) diff --git a/sim/src/core/JointManager.cpp b/sim/src/core/JointManager.cpp index 8a207a7f8..48f16fadf 100644 --- a/sim/src/core/JointManager.cpp +++ b/sim/src/core/JointManager.cpp @@ -30,8 +30,8 @@ #include "SimJoint.h" #include "JointManager.h" #include "NodeManager.h" -#include "PhysicsMapper.h" - +//#include "PhysicsMapper.h" +#include #include @@ -44,7 +44,7 @@ #include namespace mars { - namespace sim { +namespace sim { using namespace utils; using namespace interfaces; @@ -84,8 +84,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 +106,21 @@ 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); + //newJointInterface = PhysicsMapper::newJointPhysics(control->sim->getPhysics()); + std::cout << "DEBUGGG: create ODEObject pointer with odeJointFactory in JointManager " << __FILE__ << ":" << __LINE__ << std::endl; + // create the physical node data - if (newJointInterface->createJoint(jointS, i_node1, i_node2)) { + if (newJointInterface.get() == nullptr) { + std::cerr << "JointManager: Could not create new joint (ODEJoint::isJointCreated() returned false)." << std::endl; + // 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 +143,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 +151,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 +591,5 @@ namespace mars { } } - } // end of namespace sim +} // end of namespace sim } // end of namespace mars diff --git a/sim/src/physics/CoreODEJoints.cpp b/sim/src/physics/CoreODEJoints.cpp index 00be2eaf1..06d569897 100644 --- a/sim/src/physics/CoreODEJoints.cpp +++ b/sim/src/physics/CoreODEJoints.cpp @@ -31,13 +31,13 @@ namespace sim { const std::shared_ptr node1, const std::shared_ptr node2) : ODEJoint(world) { - createJoint(joint, node1, node2 ); + createJoint(joint, node1, node2 ); } ODEHingeJoint::~ODEHingeJoint(void) { } - bool ODEHingeJoint::createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2){ + bool ODEHingeJoint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2){ jointId= dJointCreateHinge(theWorld->getWorld(),0); dJointAttach(jointId, body1, body2); @@ -84,7 +84,7 @@ namespace sim { ODEHinge2Joint::~ODEHinge2Joint(void) { } - bool ODEHinge2Joint::createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2){ + bool ODEHinge2Joint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2){ if (body1 || body2){ @@ -135,7 +135,7 @@ namespace sim { ODESliderJoint::~ODESliderJoint(void) { } - bool ODESliderJoint::createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2){ + bool ODESliderJoint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2){ jointId= dJointCreateSlider(theWorld->getWorld(),0); dJointAttach(jointId, body1, body2); @@ -174,7 +174,7 @@ namespace sim { ODEBallJoint::~ODEBallJoint(void) { } - bool ODEBallJoint::createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2){ + bool ODEBallJoint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2){ jointId= dJointCreateBall(theWorld->getWorld(),0); dJointAttach(jointId, body1, body2); @@ -227,7 +227,7 @@ namespace sim { ODEUniversalJoint::~ODEUniversalJoint(void) { } - bool ODEUniversalJoint::createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2) { + bool ODEUniversalJoint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) { if (body1 || body2){ jointId= dJointCreateUniversal(theWorld->getWorld(),0); dJointAttach(jointId, body1, body2); @@ -284,7 +284,7 @@ namespace sim { ODEFixedJoint::~ODEFixedJoint(void) { } - bool ODEFixedJoint::createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2){ + bool ODEFixedJoint::createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2){ if (body1 || body2){ CPP_UNUSED(jointS); jointId = dJointCreateFixed(theWorld->getWorld(), 0); diff --git a/sim/src/physics/CoreODEJoints.h b/sim/src/physics/CoreODEJoints.h index 156b088c2..bcb8da5e1 100644 --- a/sim/src/physics/CoreODEJoints.h +++ b/sim/src/physics/CoreODEJoints.h @@ -51,7 +51,7 @@ namespace sim { const std::shared_ptr node1, const std::shared_ptr node2); virtual ~ODEHingeJoint(void); - virtual bool createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2) override; + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; }; class ODEHinge2Joint : public ODEJoint { @@ -61,7 +61,7 @@ namespace sim { const std::shared_ptr node1, const std::shared_ptr node2); virtual ~ODEHinge2Joint(void); - virtual bool createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2) override; + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; }; class ODESliderJoint : public ODEJoint { @@ -71,7 +71,7 @@ namespace sim { const std::shared_ptr node1, const std::shared_ptr node2); virtual ~ODESliderJoint(void); - virtual bool createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2) override; + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; }; class ODEBallJoint : public ODEJoint { @@ -81,7 +81,7 @@ namespace sim { const std::shared_ptr node1, const std::shared_ptr node2); virtual ~ODEBallJoint(void); - virtual bool createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2) override; + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; }; class ODEUniversalJoint : public ODEJoint { @@ -91,7 +91,7 @@ namespace sim { const std::shared_ptr node1, const std::shared_ptr node2); virtual ~ODEUniversalJoint(void); - virtual bool createODEJoint(interfaces::NodeData *node) override; + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; }; class ODEFixedJoint : public ODEJoint { @@ -101,7 +101,7 @@ namespace sim { const std::shared_ptr node1, const std::shared_ptr node2); virtual ~ODEFixedJoint(void); - virtual bool createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2) override; + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; }; } // end of namespace sim diff --git a/sim/src/physics/CoreODEObjects.cpp b/sim/src/physics/CoreODEObjects.cpp index 7238ee014..67699cba2 100644 --- a/sim/src/physics/CoreODEObjects.cpp +++ b/sim/src/physics/CoreODEObjects.cpp @@ -28,7 +28,7 @@ namespace sim { using namespace interfaces; ODEBox::ODEBox(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - createNode(nodeData); // pass a function pointer? + createNode(nodeData); } ODEBox::~ODEBox(void) { @@ -66,7 +66,7 @@ namespace sim { } ODECapsule::ODECapsule(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - createNode(nodeData); // pass a function pointer? + createNode(nodeData); } ODECapsule::~ODECapsule(void) { diff --git a/sim/src/physics/JointFactoryInterface.h b/sim/src/physics/JointFactoryInterface.h index 167b93b38..6186155e8 100644 --- a/sim/src/physics/JointFactoryInterface.h +++ b/sim/src/physics/JointFactoryInterface.h @@ -26,8 +26,8 @@ */ #pragma once - -#include +#include "WorldPhysics.h" +#include namespace mars{ namespace sim{ @@ -36,7 +36,8 @@ using namespace ::mars::interfaces; class JointFactoryInterface{ public: - virtual std::shared_ptr createJoint(interfaces::JointData *joint, + virtual std::shared_ptr createJoint(std::shared_ptr worldPhysics, + interfaces::JointData *joint, const std::shared_ptr node1, const std::shared_ptr node2) = 0; }; diff --git a/sim/src/physics/ODEJoint.cpp b/sim/src/physics/ODEJoint.cpp index d139055c8..04b372e27 100644 --- a/sim/src/physics/ODEJoint.cpp +++ b/sim/src/physics/ODEJoint.cpp @@ -55,6 +55,7 @@ namespace sim { spring = 0; body1 = 0; body2 = 0; + joint_created = false; } /** @@ -103,11 +104,15 @@ namespace sim { cfm2 = (cfm2>0)?1/cfm2:0.000000000001; } + bool ODEJoint::isJointCreated(){ + return joint_created; + } + /** * \brief create the joint with the informations giving from jointS * */ - bool ODEJoint::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", @@ -133,11 +138,11 @@ namespace sim { if(n2) b2 = n2->getBody(); body1 = b1; body2 = b2; - + bool ret; ret = createODEJoint(jointS, body1, body2); if(ret == 0) { - // Error createing the joint + // Error createing the joint return 0; } @@ -145,15 +150,16 @@ namespace sim { // 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::NodeData *node){ + 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 true; + return 0; } ///get the anchor of the joint diff --git a/sim/src/physics/ODEJoint.h b/sim/src/physics/ODEJoint.h index 6a26bf073..21c3d771e 100644 --- a/sim/src/physics/ODEJoint.h +++ b/sim/src/physics/ODEJoint.h @@ -44,7 +44,7 @@ namespace sim { virtual bool createJoint(interfaces::JointData *joint, const std::shared_ptr node1, const std::shared_ptr node2); - virtual bool createODEJoint(JointData *jointS, dBodyID body1, dBodyID body2); + virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2); ///get the anchor of the joint virtual void getAnchor(utils::Vector* anchor) const; @@ -90,6 +90,7 @@ namespace sim { virtual void setLowStop2(interfaces::sReal lowStop2); virtual void setHighStop2(interfaces::sReal highStop2); virtual void setCFM(interfaces::sReal cfm); + bool isJointCreated(); protected: std::shared_ptr theWorld; @@ -102,8 +103,8 @@ namespace sim { dReal damping, spring, jointCFM; utils::Vector axis1_torque, axis2_torque, joint_load; dReal motor_torque; - - void calculateCfmErp(const interfaces::JointData *jointS); + void calculateCfmErp(const interfaces::JointData *jointS); + bool joint_created; }; } // end of namespace sim diff --git a/sim/src/physics/ODEJointFactory.cpp b/sim/src/physics/ODEJointFactory.cpp index 1d3e9b010..b18431cdd 100644 --- a/sim/src/physics/ODEJointFactory.cpp +++ b/sim/src/physics/ODEJointFactory.cpp @@ -43,24 +43,27 @@ std::shared_ptr ODEJointFactory::createJoint(std::shared_ptr node1, const std::shared_ptr node2){ + + std::shared_ptr newJoint; + switch(jointData->type) { case JOINT_TYPE_HINGE: - return std::make_shared(worldPhysics, jointData, node1, node2); + newJoint = std::make_shared(worldPhysics, jointData, node1, node2); break; case JOINT_TYPE_HINGE2: - return std::make_shared(worldPhysics, jointData, node1, node2); + newJoint = std::make_shared(worldPhysics, jointData, node1, node2); break; case JOINT_TYPE_SLIDER: - return std::make_shared(worldPhysics, jointData, node1, node2); + newJoint = std::make_shared(worldPhysics, jointData, node1, node2); break; case JOINT_TYPE_BALL: - return std::make_shared(worldPhysics, jointData, node1, node2); + newJoint = std::make_shared(worldPhysics, jointData, node1, node2); break; case JOINT_TYPE_UNIVERSAL: - return std::make_shared(worldPhysics, jointData, node1, node2); + newJoint = std::make_shared(worldPhysics, jointData, node1, node2); break; case JOINT_TYPE_FIXED: - return std::make_shared(worldPhysics, jointData, node1, node2); + newJoint = std::make_shared(worldPhysics, jointData, node1, node2); break; default: // no correct type is spezified, so no physically node will be created @@ -68,6 +71,14 @@ std::shared_ptr ODEJointFactory::createJoint(std::shared_ptr(nullptr); break; } + + if(newJoint->isJointCreated()){ + return newJoint; + } + else{ + return std::shared_ptr(nullptr); + } + } } } diff --git a/sim/src/physics/ODEJointFactory.h b/sim/src/physics/ODEJointFactory.h index ed5ec5bac..380a05349 100644 --- a/sim/src/physics/ODEJointFactory.h +++ b/sim/src/physics/ODEJointFactory.h @@ -37,7 +37,8 @@ using namespace ::mars::interfaces; class ODEJointFactory : public JointFactoryInterface{ public: static ODEJointFactory& Instance(); - virtual std::shared_ptr createJoint(interfaces::JointData *joint, + virtual std::shared_ptr createJoint(std::shared_ptr worldPhysics, + interfaces::JointData *joint, const std::shared_ptr node1, const std::shared_ptr node2) override; From fb7f74fd267ac97c86b438f31f0c622384c3c951 Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Mon, 15 Aug 2022 18:45:55 +0200 Subject: [PATCH 14/21] Check if the ODEObject is created or not and remove JointsMapper --- sim/src/core/JointManager.cpp | 2 +- sim/src/core/PhysicsMapper.cpp | 11 +++-------- sim/src/core/PhysicsMapper.h | 2 -- sim/src/physics/ODEJoint.h | 2 ++ sim/src/physics/ODEObject.cpp | 6 ++++++ sim/src/physics/ODEObject.h | 4 ++++ sim/src/physics/ODEObjectFactory.cpp | 25 ++++++++++++++++++------- 7 files changed, 34 insertions(+), 18 deletions(-) diff --git a/sim/src/core/JointManager.cpp b/sim/src/core/JointManager.cpp index 48f16fadf..ffa7d8e50 100644 --- a/sim/src/core/JointManager.cpp +++ b/sim/src/core/JointManager.cpp @@ -109,7 +109,7 @@ namespace sim { // create an interface object to the physics newJointInterface = ODEJointFactory::Instance().createJoint(control->sim->getPhysics(), jointS, i_node1, i_node2); //newJointInterface = PhysicsMapper::newJointPhysics(control->sim->getPhysics()); - std::cout << "DEBUGGG: create ODEObject pointer with odeJointFactory in JointManager " << __FILE__ << ":" << __LINE__ << std::endl; + std::cout << "DEBUGGG: create ODEJoint pointer with odeJointFactory in JointManager " << __FILE__ << ":" << __LINE__ << std::endl; // create the physical node data if (newJointInterface.get() == nullptr) { diff --git a/sim/src/core/PhysicsMapper.cpp b/sim/src/core/PhysicsMapper.cpp index e493761d5..80f9533ba 100644 --- a/sim/src/core/PhysicsMapper.cpp +++ b/sim/src/core/PhysicsMapper.cpp @@ -29,19 +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 worldPhysics = std::make_shared(control); return std::static_pointer_cast(worldPhysics); - } + } - 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 0c41bc72b..6353b9f5f 100644 --- a/sim/src/core/PhysicsMapper.h +++ b/sim/src/core/PhysicsMapper.h @@ -34,7 +34,6 @@ #endif #include "WorldPhysics.h" -#include "JointPhysics.h" namespace mars { namespace sim { @@ -42,7 +41,6 @@ namespace mars { class PhysicsMapper { public: static std::shared_ptr newWorldPhysics(interfaces::ControlCenter *control); - static std::shared_ptr newJointPhysics(std::shared_ptr worldPhysics); }; } // end of namespace sim diff --git a/sim/src/physics/ODEJoint.h b/sim/src/physics/ODEJoint.h index 21c3d771e..bf49ced82 100644 --- a/sim/src/physics/ODEJoint.h +++ b/sim/src/physics/ODEJoint.h @@ -104,6 +104,8 @@ namespace sim { utils::Vector axis1_torque, axis2_torque, joint_load; dReal motor_torque; void calculateCfmErp(const interfaces::JointData *jointS); + + private: bool joint_created; }; diff --git a/sim/src/physics/ODEObject.cpp b/sim/src/physics/ODEObject.cpp index 0e4d2857c..04df13323 100644 --- a/sim/src/physics/ODEObject.cpp +++ b/sim/src/physics/ODEObject.cpp @@ -64,6 +64,7 @@ namespace sim { //node_data.num_ground_collisions = 0; node_data.setZero(); dMassSetZero(&nMass); + object_created = false; } /** @@ -96,6 +97,10 @@ namespace sim { } } + bool ODEObject::isObjectCreated(){ + return object_created; + } + /** * \brief The method copies the position of the node at the adress * of the pointer pos. @@ -247,6 +252,7 @@ namespace sim { dGeomSetData(nGeom, &node_data); locker.unlock(); setContactParams(node->c_params); + object_created = true; return 1; } LOG_WARN("ODEBox: tried to create a Box without there being a world."); diff --git a/sim/src/physics/ODEObject.h b/sim/src/physics/ODEObject.h index bb5a6a9bf..541e66b81 100644 --- a/sim/src/physics/ODEObject.h +++ b/sim/src/physics/ODEObject.h @@ -138,6 +138,7 @@ namespace sim { dMass getODEMass(void) const; void addMassToCompositeBody(dBodyID theBody, dMass *bodyMass); void getAbsMass(dMass *pMass) const; + bool isObjectCreated(); protected: std::shared_ptr theWorld; @@ -149,6 +150,9 @@ namespace sim { std::vector sensor_list; void setProperties(interfaces::NodeData *node); void setInertiaMass(interfaces::NodeData *node); + + private: + bool object_created; }; } // end of namespace sim diff --git a/sim/src/physics/ODEObjectFactory.cpp b/sim/src/physics/ODEObjectFactory.cpp index 6a50839f8..133d59175 100644 --- a/sim/src/physics/ODEObjectFactory.cpp +++ b/sim/src/physics/ODEObjectFactory.cpp @@ -40,27 +40,30 @@ ODEObjectFactory::~ODEObjectFactory(){ } std::shared_ptr ODEObjectFactory::createObject(std::shared_ptr worldPhysics, NodeData * nodeData){ + + std::shared_ptr newObject; + switch(nodeData->physicMode) { case NODE_TYPE_BOX: - return std::make_shared(worldPhysics, nodeData); + newObject = std::make_shared(worldPhysics, nodeData); break; case NODE_TYPE_CAPSULE: - return std::make_shared(worldPhysics, nodeData); + newObject = std::make_shared(worldPhysics, nodeData); break; case NODE_TYPE_CYLINDER: - return std::make_shared(worldPhysics, nodeData); + newObject = std::make_shared(worldPhysics, nodeData); break; case NODE_TYPE_MESH: - return std::make_shared(worldPhysics, nodeData); + newObject = std::make_shared(worldPhysics, nodeData); break; case NODE_TYPE_PLANE: - return std::make_shared(worldPhysics, nodeData); + newObject = std::make_shared(worldPhysics, nodeData); break; case NODE_TYPE_SPHERE: - return std::make_shared(worldPhysics, nodeData); + newObject = std::make_shared(worldPhysics, nodeData); break; case NODE_TYPE_TERRAIN: - return std::make_shared(worldPhysics, nodeData); + newObject = std::make_shared(worldPhysics, nodeData); break; default: // no correct type is spezified, so no physically node will be created @@ -68,6 +71,14 @@ std::shared_ptr ODEObjectFactory::createObject(std::shared_ptr(nullptr); break; } + + if(newObject->isObjectCreated()){ + return newObject; + } + else{ + return std::shared_ptr(nullptr); + } + } } } From 9698a7e2fd3f07cc3fb3ec6bfe355bca64e08c52 Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Mon, 15 Aug 2022 18:50:09 +0200 Subject: [PATCH 15/21] Cleanup comments --- sim/src/core/JointManager.cpp | 4 +--- sim/src/core/NodeManager.cpp | 1 - sim/src/physics/CoreODEObjects.cpp | 1 + 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/sim/src/core/JointManager.cpp b/sim/src/core/JointManager.cpp index ffa7d8e50..7560e18bd 100644 --- a/sim/src/core/JointManager.cpp +++ b/sim/src/core/JointManager.cpp @@ -108,12 +108,10 @@ namespace sim { // create an interface object to the physics newJointInterface = ODEJointFactory::Instance().createJoint(control->sim->getPhysics(), jointS, i_node1, i_node2); - //newJointInterface = PhysicsMapper::newJointPhysics(control->sim->getPhysics()); - std::cout << "DEBUGGG: create ODEJoint pointer with odeJointFactory in JointManager " << __FILE__ << ":" << __LINE__ << std::endl; // create the physical node data if (newJointInterface.get() == nullptr) { - std::cerr << "JointManager: Could not create new joint (ODEJoint::isJointCreated() returned false)." << std::endl; + LOG_ERROR("JointManager::addJoint: No joint was created in physics."); // if no node was created in physics // delete the objects newJointInterface.reset(); diff --git a/sim/src/core/NodeManager.cpp b/sim/src/core/NodeManager.cpp index 1d29b36f3..f1d08f074 100644 --- a/sim/src/core/NodeManager.cpp +++ b/sim/src/core/NodeManager.cpp @@ -250,7 +250,6 @@ namespace mars { if(! (nodeS->noPhysical)){ // create an interface object to the physics std::shared_ptr newNodeInterface = ODEObjectFactory::Instance().createObject(control->sim->getPhysics(), nodeS); - std::cout << "DEBUGGG: create ODEObject pointer with odeObjectFactory in NodeManager " << __FILE__ << ":" << __LINE__ << std::endl; if (newNodeInterface.get() == nullptr) { // if no node was created in physics diff --git a/sim/src/physics/CoreODEObjects.cpp b/sim/src/physics/CoreODEObjects.cpp index 67699cba2..2bf332ee0 100644 --- a/sim/src/physics/CoreODEObjects.cpp +++ b/sim/src/physics/CoreODEObjects.cpp @@ -62,6 +62,7 @@ namespace sim { dMassSetBoxTotal(&nMass, tempMass, (dReal)(node->ext.x()), (dReal)(node->ext.y()),(dReal)(node->ext.z())); } + return false; return true; } From 084f170c268b9a44d0de2906e4a075e8ec9e165e Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Wed, 17 Aug 2022 16:11:49 +0200 Subject: [PATCH 16/21] Remove a return used during debugging --- sim/src/physics/CoreODEObjects.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/sim/src/physics/CoreODEObjects.cpp b/sim/src/physics/CoreODEObjects.cpp index 2bf332ee0..67699cba2 100644 --- a/sim/src/physics/CoreODEObjects.cpp +++ b/sim/src/physics/CoreODEObjects.cpp @@ -62,7 +62,6 @@ namespace sim { dMassSetBoxTotal(&nMass, tempMass, (dReal)(node->ext.x()), (dReal)(node->ext.y()),(dReal)(node->ext.z())); } - return false; return true; } From b26e7af3cd9df7eb57a477a74a0bc121eaba4f56 Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Fri, 30 Dec 2022 10:41:55 +0100 Subject: [PATCH 17/21] Split CoreODEObjects & CoreODEJoints into separate files --- sim/CMakeLists.txt | 56 +- sim/src/core/JointManager.cpp | 5 +- sim/src/core/NodeManager.cpp | 7 +- sim/src/physics/CoreODEJoints.cpp | 305 --- sim/src/physics/CoreODEJoints.h | 110 - sim/src/physics/CoreODEObjects.cpp | 348 --- sim/src/physics/CoreODEObjects.h | 112 - sim/src/physics/JointPhysics.cpp | 1448 ------------- sim/src/physics/JointPhysics.h | 124 -- sim/src/physics/NodePhysics.cpp | 1879 ----------------- sim/src/physics/NodePhysics.h | 171 -- sim/src/physics/WorldPhysics.cpp | 2 +- .../{ => joints}/JointFactoryInterface.h | 0 sim/src/physics/joints/ODEBallJoint.cpp | 83 + sim/src/physics/joints/ODEBallJoint.h | 59 + sim/src/physics/joints/ODEFixedJoint.cpp | 58 + sim/src/physics/joints/ODEFixedJoint.h | 59 + sim/src/physics/joints/ODEHinge2Joint.cpp | 81 + sim/src/physics/joints/ODEHinge2Joint.h | 59 + sim/src/physics/joints/ODEHingeJoint.cpp | 77 + sim/src/physics/joints/ODEHingeJoint.h | 59 + sim/src/physics/{ => joints}/ODEJoint.cpp | 2 +- sim/src/physics/{ => joints}/ODEJoint.h | 0 .../physics/{ => joints}/ODEJointFactory.cpp | 27 +- .../physics/{ => joints}/ODEJointFactory.h | 2 +- sim/src/physics/joints/ODESliderJoint.cpp | 69 + sim/src/physics/joints/ODESliderJoint.h | 59 + sim/src/physics/joints/ODEUniversalJoint.cpp | 87 + sim/src/physics/joints/ODEUniversalJoint.h | 59 + sim/src/physics/objects/ODEBox.cpp | 69 + sim/src/physics/objects/ODEBox.h | 58 + sim/src/physics/objects/ODECapsule.cpp | 67 + sim/src/physics/objects/ODECapsule.h | 56 + sim/src/physics/objects/ODECylinder.cpp | 67 + sim/src/physics/objects/ODECylinder.h | 56 + sim/src/physics/objects/ODEHeightField.cpp | 89 + sim/src/physics/objects/ODEHeightField.h | 62 + sim/src/physics/objects/ODEMesh.cpp | 131 ++ sim/src/physics/objects/ODEMesh.h | 61 + sim/src/physics/{ => objects}/ODEObject.cpp | 0 sim/src/physics/{ => objects}/ODEObject.h | 0 .../{ => objects}/ODEObjectFactory.cpp | 24 +- .../physics/{ => objects}/ODEObjectFactory.h | 2 +- sim/src/physics/objects/ODEPlane.cpp | 44 + sim/src/physics/objects/ODEPlane.h | 56 + sim/src/physics/objects/ODESphere.cpp | 67 + sim/src/physics/objects/ODESphere.h | 56 + .../{ => objects}/ObjectFactoryInterface.h | 0 48 files changed, 1827 insertions(+), 4545 deletions(-) delete mode 100644 sim/src/physics/CoreODEJoints.cpp delete mode 100644 sim/src/physics/CoreODEJoints.h delete mode 100644 sim/src/physics/CoreODEObjects.cpp delete mode 100644 sim/src/physics/CoreODEObjects.h delete mode 100644 sim/src/physics/JointPhysics.cpp delete mode 100644 sim/src/physics/JointPhysics.h delete mode 100644 sim/src/physics/NodePhysics.cpp delete mode 100644 sim/src/physics/NodePhysics.h rename sim/src/physics/{ => joints}/JointFactoryInterface.h (100%) create mode 100644 sim/src/physics/joints/ODEBallJoint.cpp create mode 100644 sim/src/physics/joints/ODEBallJoint.h create mode 100644 sim/src/physics/joints/ODEFixedJoint.cpp create mode 100644 sim/src/physics/joints/ODEFixedJoint.h create mode 100644 sim/src/physics/joints/ODEHinge2Joint.cpp create mode 100644 sim/src/physics/joints/ODEHinge2Joint.h create mode 100644 sim/src/physics/joints/ODEHingeJoint.cpp create mode 100644 sim/src/physics/joints/ODEHingeJoint.h rename sim/src/physics/{ => joints}/ODEJoint.cpp (99%) rename sim/src/physics/{ => joints}/ODEJoint.h (100%) rename sim/src/physics/{ => joints}/ODEJointFactory.cpp (85%) rename sim/src/physics/{ => joints}/ODEJointFactory.h (97%) create mode 100644 sim/src/physics/joints/ODESliderJoint.cpp create mode 100644 sim/src/physics/joints/ODESliderJoint.h create mode 100644 sim/src/physics/joints/ODEUniversalJoint.cpp create mode 100644 sim/src/physics/joints/ODEUniversalJoint.h create mode 100644 sim/src/physics/objects/ODEBox.cpp create mode 100644 sim/src/physics/objects/ODEBox.h create mode 100644 sim/src/physics/objects/ODECapsule.cpp create mode 100644 sim/src/physics/objects/ODECapsule.h create mode 100644 sim/src/physics/objects/ODECylinder.cpp create mode 100644 sim/src/physics/objects/ODECylinder.h create mode 100644 sim/src/physics/objects/ODEHeightField.cpp create mode 100644 sim/src/physics/objects/ODEHeightField.h create mode 100644 sim/src/physics/objects/ODEMesh.cpp create mode 100644 sim/src/physics/objects/ODEMesh.h rename sim/src/physics/{ => objects}/ODEObject.cpp (100%) rename sim/src/physics/{ => objects}/ODEObject.h (100%) rename sim/src/physics/{ => objects}/ODEObjectFactory.cpp (87%) rename sim/src/physics/{ => objects}/ODEObjectFactory.h (96%) create mode 100644 sim/src/physics/objects/ODEPlane.cpp create mode 100644 sim/src/physics/objects/ODEPlane.h create mode 100644 sim/src/physics/objects/ODESphere.cpp create mode 100644 sim/src/physics/objects/ODESphere.h rename sim/src/physics/{ => objects}/ObjectFactoryInterface.h (100%) diff --git a/sim/CMakeLists.txt b/sim/CMakeLists.txt index fb36c077f..6cf05f527 100644 --- a/sim/CMakeLists.txt +++ b/sim/CMakeLists.txt @@ -86,17 +86,29 @@ set(SOURCES_H src/core/Simulator.h src/sensors/RotatingRaySensor.h - #src/physics/JointPhysics.h - src/physics/ODEObject.h - src/physics/ODEJoint.h src/physics/WorldPhysics.h src/physics/ContactsPhysics.hpp - src/physics/ODEObjectFactory.h - src/physics/ObjectFactoryInterface.h - src/physics/CoreODEObjects.h - src/physics/ODEJointFactory.h - src/physics/JointFactoryInterface.h - src/physics/CoreODEJoints.h + + 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 @@ -145,14 +157,26 @@ set(TARGET_SRC src/sensors/MultiLevelLaserRangeFinder.cpp src/sensors/RotatingRaySensor.cpp - #src/physics/JointPhysics.cpp - src/physics/ODEJoint.cpp - src/physics/ODEObject.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/physics/ODEObjectFactory.cpp - src/physics/CoreODEObjects.cpp - src/physics/ODEJointFactory.cpp - src/physics/CoreODEJoints.cpp src/sensors/CameraSensor.cpp src/sensors/Joint6DOFSensor.cpp diff --git a/sim/src/core/JointManager.cpp b/sim/src/core/JointManager.cpp index 7560e18bd..bb30b94db 100644 --- a/sim/src/core/JointManager.cpp +++ b/sim/src/core/JointManager.cpp @@ -30,8 +30,7 @@ #include "SimJoint.h" #include "JointManager.h" #include "NodeManager.h" -//#include "PhysicsMapper.h" -#include +#include "joints/ODEJointFactory.h" #include @@ -107,7 +106,7 @@ namespace sim { } // create an interface object to the physics - newJointInterface = ODEJointFactory::Instance().createJoint(control->sim->getPhysics(), jointS, i_node1, i_node2); + newJointInterface = ODEJointFactory::Instance().createJoint(control->sim->getPhysics(), jointS, i_node1, i_node2); // create the physical node data if (newJointInterface.get() == nullptr) { diff --git a/sim/src/core/NodeManager.cpp b/sim/src/core/NodeManager.cpp index f1d08f074..f06c4427e 100644 --- a/sim/src/core/NodeManager.cpp +++ b/sim/src/core/NodeManager.cpp @@ -29,7 +29,7 @@ #include "SimJoint.h" #include "NodeManager.h" #include "JointManager.h" -#include "PhysicsMapper.h" +#include "objects/ODEObjectFactory.h" #include #include @@ -47,9 +47,6 @@ #include -#include -#include - namespace mars { namespace sim { @@ -249,7 +246,7 @@ namespace mars { // create the physical node data if(! (nodeS->noPhysical)){ // create an interface object to the physics - std::shared_ptr newNodeInterface = ODEObjectFactory::Instance().createObject(control->sim->getPhysics(), nodeS); + std::shared_ptr newNodeInterface = ODEObjectFactory::Instance().createObject(control->sim->getPhysics(), nodeS); if (newNodeInterface.get() == nullptr) { // if no node was created in physics diff --git a/sim/src/physics/CoreODEJoints.cpp b/sim/src/physics/CoreODEJoints.cpp deleted file mode 100644 index 06d569897..000000000 --- a/sim/src/physics/CoreODEJoints.cpp +++ /dev/null @@ -1,305 +0,0 @@ -/* - * 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 "CoreODEJoints.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) { - } - - 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; - } - - 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) { - } - - 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; - } - - 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) { - } - - 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; - } - - 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) { - } - - 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; - } - - 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) { - } - - 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; - } - - 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) { - } - - 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/CoreODEJoints.h b/sim/src/physics/CoreODEJoints.h deleted file mode 100644 index bcb8da5e1..000000000 --- a/sim/src/physics/CoreODEJoints.h +++ /dev/null @@ -1,110 +0,0 @@ -/* - * 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 CoreODEJoints.h - * \author Malte Roemmermann, Muhammad Haider Khan Lodhi - * \brief A collection of OBE joint classes - * - */ - -#ifndef CORE_ODE_OBJECTS_H -#define CORE_ODE_OBJECTS_H - -#ifdef _PRINT_HEADER_ - #warning "CoreODEJoints.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); - virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; - }; - - 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); - virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; - }; - - 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); - virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; - }; - - 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); - virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; - }; - - 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); - virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; - }; - - 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); - virtual bool createODEJoint(interfaces::JointData *jointS, dBodyID body1, dBodyID body2) override; - }; - -} // end of namespace sim -} // end of namespace mars - -#endif // CORE_ODE_JOINTS_H diff --git a/sim/src/physics/CoreODEObjects.cpp b/sim/src/physics/CoreODEObjects.cpp deleted file mode 100644 index 67699cba2..000000000 --- a/sim/src/physics/CoreODEObjects.cpp +++ /dev/null @@ -1,348 +0,0 @@ -/* - * 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 "CoreODEObjects.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) { - } - - 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; - } - - ODECapsule::ODECapsule(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - createNode(nodeData); - } - - ODECapsule::~ODECapsule(void) { - } - - 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; - } - - ODECylinder::ODECylinder(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - createNode(nodeData); - } - - ODECylinder::~ODECylinder(void) { - } - - 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; - } - - 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); - } - - 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; - } - - 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); - } - - 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; - } - - ODEPlane::ODEPlane(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - createNode(nodeData); - } - - ODEPlane::~ODEPlane(void) { - } - - bool ODEPlane::createODEGeometry(interfaces::NodeData *node){ - // build the ode representation - nGeom = dCreatePlane(theWorld->getSpace(), 0, 0, 1, (dReal)node->pos.z()); - return true; - } - - ODESphere::ODESphere(std::shared_ptr world, NodeData * nodeData) : ODEObject(world, nodeData) { - createNode(nodeData); - } - - ODESphere::~ODESphere(void) { - } - - /** - * 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/CoreODEObjects.h b/sim/src/physics/CoreODEObjects.h deleted file mode 100644 index bf1946b35..000000000 --- a/sim/src/physics/CoreODEObjects.h +++ /dev/null @@ -1,112 +0,0 @@ -/* - * 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 CoreODEObjects.h - * \author Malte Roemmermann, Muhammad Haider Khan Lodhi - * \brief A collection of OBE object classes - * - */ - -#ifndef CORE_ODE_OBJECTS_H -#define CORE_ODE_OBJECTS_H - -#ifdef _PRINT_HEADER_ - #warning "CoreODEObjects.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); - //TODO createGeometry vs createCollision? nBody vs nCollision - // review header comment on ODEBox - virtual bool createODEGeometry(interfaces::NodeData *node) override; - }; - - class ODECapsule : public ODEObject { - public: - ODECapsule(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODECapsule(void); - virtual bool createODEGeometry(interfaces::NodeData *node) override; - }; - - class ODECylinder : public ODEObject { - public: - ODECylinder(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODECylinder(void); - virtual bool createODEGeometry(interfaces::NodeData *node) override; - }; - - class ODEHeightField : public ODEObject { - public: - ODEHeightField(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODEHeightField(void); - 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; - }; - - class ODEMesh : public ODEObject { - public: - ODEMesh(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODEMesh(void); - virtual bool createODEGeometry(interfaces::NodeData *node) override; - virtual void destroyNode(void) override; - protected: - dVector3 *myVertices; - dTriIndex *myIndices; - dTriMeshDataID myTriMeshData; - }; - - class ODEPlane : public ODEObject { - public: - ODEPlane(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODEPlane(void); - virtual bool createODEGeometry(interfaces::NodeData *node) override; - }; - - class ODESphere : public ODEObject { - public: - ODESphere(std::shared_ptr world, interfaces::NodeData * nodeData); - virtual ~ODESphere(void); - virtual bool createODEGeometry(interfaces::NodeData *node) override; - }; - -} // end of namespace sim -} // end of namespace mars - -#endif // CORE_ODE_OBJECTS_H diff --git a/sim/src/physics/JointPhysics.cpp b/sim/src/physics/JointPhysics.cpp deleted file mode 100644 index 62a32ffca..000000000 --- a/sim/src/physics/JointPhysics.cpp +++ /dev/null @@ -1,1448 +0,0 @@ -/* - * 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 JointPhysics.h - * \autor Larbi Abdenebaoui - * \brief "JointPhysics" declares the physics for the joint using ode - * - * core "SimJoint" and the physics "JointPhysics" - * SimJoint ------> JointInterface(i_Joint) ------------> JointPhysics - * - */ - -#include - -#include "JointPhysics.h" -#include "ODEObject.h" - -#include - -namespace mars { - namespace sim { - - using namespace utils; - using namespace interfaces; - - /** - * \brief the constructor of the Joint physics - * initialize the attributes of the object - */ - JointPhysics::JointPhysics(std::shared_ptr world){ - theWorld = std::static_pointer_cast(world); - jointId = ball_motor = 0; - jointCFM = 0.0; - cfm = cfm1 = cfm2 = erp1 = erp2 = 0; - lo1 = lo2 = hi1 = hi2 = 0; - damping = 0; - spring = 0; - body1 = 0; - body2 = 0; - } - - /** - * \brief Destroys the joint in the physics world. - * - * pre: - * - theWorld is the correct world object - * - * post: - * - all physical representation of the joint should be cleared - */ - JointPhysics::~JointPhysics(void) { - MutexLocker locker(&(theWorld->iMutex)); - if (jointId) { - dJointDestroy(jointId); - } - } - - - void JointPhysics::calculateCfmErp(const JointData *jointS) { - // how to set erp and cfm - // ERP = h kp / (h kp + kd) - // CFM = 1 / (h kp + kd) - damping = (dReal)jointS->damping_const_constraint_axis1; - spring = (dReal)jointS->spring_const_constraint_axis1; - dReal h = theWorld->getWorldStep(); - cfm = damping; - erp1 = h*(dReal)jointS->spring_const_constraint_axis1 - +(dReal)jointS->damping_const_constraint_axis1; - cfm1 = h*(dReal)jointS->spring_const_constraint_axis1 - +(dReal)jointS->damping_const_constraint_axis1; - erp2 = h*(dReal)jointS->spring_const_constraint_axis2 - +(dReal)jointS->damping_const_constraint_axis2; - cfm2 = h*(dReal)jointS->spring_const_constraint_axis2 - +(dReal)jointS->damping_const_constraint_axis2; - - lo1 = jointS->lowStopAxis1; - hi1 = jointS->highStopAxis1; - lo2 = jointS->lowStopAxis2; - hi2 = jointS->highStopAxis2; - // we don't want to run in the trap where kp and kd are set to zero - cfm = (cfm>0)?1/cfm:0.000000000001; - erp1 = (erp1>0)?h*(dReal)jointS->spring_const_constraint_axis1/erp1:0; - cfm1 = (cfm1>0)?1/cfm1:0.00000000001; - erp2 = (erp2>0)?h*(dReal)jointS->spring_const_constraint_axis2/erp2:0; - cfm2 = (cfm2>0)?1/cfm2:0.000000000001; - } - - /** - * \brief create the joint with the informations giving from jointS - * - */ - bool JointPhysics::createJoint(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", - jointS->index, jointS->nodeIndex1, jointS->nodeIndex2, - jointS->anchor.x(), jointS->anchor.y(), jointS->anchor.z(), - jointS->axis1.x(), jointS->axis1.y(), jointS->axis1.z()); -#endif - MutexLocker locker(&(theWorld->iMutex)); - 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); - dBodyID b1 = 0, b2 = 0; - - calculateCfmErp(jointS); - if(jointS->config.hasKey("jointCFM")) { - jointCFM = jointS->config["jointCFM"]; - } - - joint_type = jointS->type; - if(n1) b1 = n1->getBody(); - 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; - 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); - return 1; - } - return 0; - } - - ///get the anchor of the joint - void JointPhysics::getAnchor(Vector* anchor) const { - dReal pos[4] = {0,0,0,0}; - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointGetHingeAnchor(jointId, pos); - break; - case JOINT_TYPE_HINGE2: - dJointGetHinge2Anchor(jointId, pos); - break; - case JOINT_TYPE_SLIDER: - // the slider joint has no ancher point - break; - case JOINT_TYPE_BALL: - dJointGetBallAnchor(jointId, pos); - break; - case JOINT_TYPE_UNIVERSAL: - dJointGetUniversalAnchor(jointId, pos); - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - anchor->x() = pos[0]; - anchor->y() = pos[1]; - anchor->z() = pos[2]; - } - - // the next force and velocity methods are only in a beta state - void JointPhysics::setForceLimit(sReal max_force) { - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointSetHingeParam(jointId, dParamFMax, (dReal)max_force); - break; - case JOINT_TYPE_HINGE2: - dJointSetHinge2Param(jointId, dParamFMax, (dReal)max_force); - break; - case JOINT_TYPE_SLIDER: - dJointSetSliderParam(jointId, dParamFMax, (dReal)max_force); - break; - case JOINT_TYPE_UNIVERSAL: - dJointSetUniversalParam(jointId, dParamFMax, (dReal)max_force); - break; - } - } - - void JointPhysics::setForceLimit2(sReal max_force) { - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_HINGE: - break; - case JOINT_TYPE_HINGE2: - dJointSetHinge2Param(jointId, dParamFMax2, (dReal)max_force); - break; - case JOINT_TYPE_SLIDER: - break; - case JOINT_TYPE_UNIVERSAL: - dJointSetUniversalParam(jointId, dParamFMax2, (dReal)max_force); - break; - } - } - - void JointPhysics::setVelocity(sReal velocity) { - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointSetHingeParam(jointId, dParamVel, (dReal)velocity); - break; - case JOINT_TYPE_HINGE2: - dJointSetHinge2Param(jointId, dParamVel, (dReal)velocity); - break; - case JOINT_TYPE_SLIDER: - dJointSetSliderParam(jointId, dParamVel, (dReal)velocity); - break; - case JOINT_TYPE_UNIVERSAL: - dJointSetUniversalParam(jointId, dParamVel, (dReal)velocity); - break; - } - } - - void JointPhysics::setVelocity2(sReal velocity) { - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_HINGE: - break; - case JOINT_TYPE_HINGE2: - dJointSetHinge2Param(jointId, dParamVel2, (dReal)velocity); - break; - case JOINT_TYPE_SLIDER: - break; - case JOINT_TYPE_UNIVERSAL: - dJointSetUniversalParam(jointId, dParamVel2, (dReal)velocity); - break; - } - } - - sReal JointPhysics::getPosition(void) const { - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_HINGE: - return (sReal)dJointGetHingeAngle(jointId); - break; - case JOINT_TYPE_HINGE2: - return (sReal)dJointGetHinge2Angle1(jointId); - break; - case JOINT_TYPE_SLIDER: - return (sReal)dJointGetSliderPosition(jointId); - break; - case JOINT_TYPE_UNIVERSAL: - return (sReal)dJointGetUniversalAngle1(jointId); - break; - } - return 0; - } - - sReal JointPhysics::getPosition2(void) const { - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_UNIVERSAL: - return (sReal)dJointGetUniversalAngle2(jointId); - break; - } - 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){ - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointSetHingeAnchor(jointId, anchor.x(), anchor.y(), anchor.z()); - //std::cout << " " << anchor.z(); - break; - case JOINT_TYPE_HINGE2: - dJointSetHinge2Anchor(jointId, anchor.x(), anchor.y(), anchor.z()); - break; - case JOINT_TYPE_SLIDER: - // the slider joint has no ancher point - break; - case JOINT_TYPE_BALL: - dJointSetBallAnchor(jointId, anchor.x(), anchor.y(), anchor.z()); - break; - case JOINT_TYPE_UNIVERSAL: - dJointSetUniversalAnchor(jointId, anchor.x(), anchor.y(), anchor.z()); - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - } - - /** - * \brief Set the Axis of the Joint to a new position - * - * pre: - * - * post: - */ - void JointPhysics::setAxis(const Vector &axis){ - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointSetHingeAxis(jointId, axis.x(), axis.y(), axis.z()); - break; - case JOINT_TYPE_HINGE2: - dJointSetHinge2Axis1(jointId, axis.x(), axis.y(), axis.z()); - break; - case JOINT_TYPE_SLIDER: - dJointSetSliderAxis(jointId, axis.x(), axis.y(), axis.z()); - break; - case JOINT_TYPE_BALL: - // the ball joint has no axis - break; - case JOINT_TYPE_UNIVERSAL: - dJointSetUniversalAxis1(jointId, axis.x(), axis.y(), axis.z()); - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - } - - /** - * \brief Set the Axis2 of the Joint - * - * pre: - * - * post: - */ - void JointPhysics::setAxis2(const Vector &axis){ - MutexLocker locker(&(theWorld->iMutex)); - switch(joint_type) { - case JOINT_TYPE_HINGE: - // the hinge joint has only one axis - break; - case JOINT_TYPE_HINGE2: - dJointSetHinge2Axis2(jointId, axis.x(), axis.y(), axis.z()); - break; - case JOINT_TYPE_SLIDER: - // the slider joint has only one axis - break; - case JOINT_TYPE_BALL: - // the ball joint has no axis - break; - case JOINT_TYPE_UNIVERSAL: - dJointSetUniversalAxis2(jointId, axis.x(), axis.y(), axis.z()); - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - } - - /** - * \brief Gets the actual axis of a joint - * - * pre: - * - the joint should be created and should have a axis vector - * - * post: - * - the given axis struct should be filled with correct values - */ - void JointPhysics::getAxis(Vector* axis) const { - dReal pos[4] = {0,0,0,0}; - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointGetHingeAxis(jointId, pos); - break; - case JOINT_TYPE_HINGE2: - dJointGetHinge2Axis1(jointId, pos); - break; - case JOINT_TYPE_SLIDER: - dJointGetSliderAxis(jointId, pos); - break; - case JOINT_TYPE_BALL: - // the ball joint has no axis - break; - case JOINT_TYPE_UNIVERSAL: - dJointGetUniversalAxis1(jointId, pos); - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - axis->x() = (sReal)pos[0]; - axis->y() = (sReal)pos[1]; - axis->z() = (sReal)pos[2]; - } - - /** - * \brief Gets the actual second axis of a joint - * - * pre: - * - the joint should be created and should have second axis vector - * - * post: - * - the given axis struct should be filled with correct values - */ - void JointPhysics::getAxis2(Vector* axis) const { - dReal pos[4] = {0,0,0,0}; - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_HINGE: - // the hinge joint has only one axis - break; - case JOINT_TYPE_HINGE2: - dJointGetHinge2Axis2(jointId, pos); - break; - case JOINT_TYPE_SLIDER: - // the slider joint has only one axis - break; - case JOINT_TYPE_BALL: - // the ball joint has no axis - break; - case JOINT_TYPE_UNIVERSAL: - dJointGetUniversalAxis2(jointId, pos); - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - axis->x() = (sReal)pos[0]; - axis->y() = (sReal)pos[1]; - axis->z() = (sReal)pos[2]; - } - - ///set the world informations - void JointPhysics::setWorldObject(std::shared_ptr world){ - theWorld = std::static_pointer_cast(world); - } - - void JointPhysics::setJointAsMotor(int axis) { - MutexLocker locker(&(theWorld->iMutex)); - switch(joint_type) { - // todo: need to handle the distinction whether to set or not to - // set the hi and low stop differently - case JOINT_TYPE_HINGE: - //if(!lo1 && !hi) - { - dJointSetHingeParam(jointId, dParamLoStop, -dInfinity); - dJointSetHingeParam(jointId, dParamHiStop, dInfinity); - } - break; - case JOINT_TYPE_HINGE2: - if(!lo1 && !hi1 && axis == 1) { - dJointSetHinge2Param(jointId, dParamLoStop, -dInfinity); - dJointSetHinge2Param(jointId, dParamHiStop, dInfinity); - } - if(!lo2 && !hi2 && axis == 2) { - dJointSetHinge2Param(jointId, dParamLoStop2, -dInfinity); - dJointSetHinge2Param(jointId, dParamHiStop2, dInfinity); - } - break; - case JOINT_TYPE_SLIDER: - if(!lo1 && !hi1) { - dJointSetSliderParam(jointId, dParamLoStop, -dInfinity); - dJointSetSliderParam(jointId, dParamHiStop, dInfinity); - } - break; - case JOINT_TYPE_UNIVERSAL: - /* - if(!lo1 && !hi1 && axis == 1) { - dJointSetUniversalParam(jointId, dParamLoStop, -dInfinity); - dJointSetUniversalParam(jointId, dParamHiStop, dInfinity); - } - if(!lo2 && !hi2 && axis == 2) { - dJointSetUniversalParam(jointId, dParamLoStop2, -dInfinity); - dJointSetUniversalParam(jointId, dParamHiStop2, dInfinity); - } - */ - break; - } - } - - void JointPhysics::unsetJointAsMotor(int axis) { - MutexLocker locker(&(theWorld->iMutex)); - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointSetHingeParam(jointId, dParamLoStop, lo1); - dJointSetHingeParam(jointId, dParamHiStop, hi1); - break; - case JOINT_TYPE_HINGE2: - if(axis == 1) { - dJointSetHinge2Param(jointId, dParamLoStop, lo1); - dJointSetHinge2Param(jointId, dParamHiStop, hi1); - } - else if(axis == 2) { - dJointSetHinge2Param(jointId, dParamLoStop2, lo2); - dJointSetHinge2Param(jointId, dParamHiStop2, hi2); - } - break; - case JOINT_TYPE_SLIDER: - dJointSetSliderParam(jointId, dParamLoStop, lo1); - dJointSetSliderParam(jointId, dParamHiStop, hi1); - break; - case JOINT_TYPE_UNIVERSAL: - if(axis == 1) { - dJointSetUniversalParam(jointId, dParamLoStop, lo1); - dJointSetUniversalParam(jointId, dParamHiStop, hi1); - } - else if(axis == 2) { - dJointSetUniversalParam(jointId, dParamLoStop2, lo2); - dJointSetUniversalParam(jointId, dParamHiStop2, hi2); - } - break; - } - } - - /** - * \brief Gets the force the joint applies to the first body - * - * pre: - * - the joint should be created - * - * post: - * - */ - void JointPhysics::getForce1(Vector *f) const { - f->x() = (sReal)feedback.f1[0]; - f->y() = (sReal)feedback.f1[1]; - f->z() = (sReal)feedback.f1[2]; - } - - /** - * \brief Gets the force the joint applies to the second body - * - * pre: - * - the joint should be created - * - * post: - * - */ - void JointPhysics::getForce2(Vector *f) const { - f->x() = (sReal)feedback.f2[0]; - f->y() = (sReal)feedback.f2[1]; - f->z() = (sReal)feedback.f2[2]; - } - - /** - * \brief Gets the torque the joint applies to the first body - * - * pre: - * - the joint should be created - * - * post: - * - */ - void JointPhysics::getTorque1(Vector *t) const { - t->x() = (sReal)feedback.t1[0]; - t->y() = (sReal)feedback.t1[1]; - t->z() = (sReal)feedback.t1[2]; - } - - /** - * \brief Gets the torque the joint applies to the second body - * - * pre: - * - the joint should be created - * - * post: - * - */ - void JointPhysics::getTorque2(Vector *t) const { - t->x() = (sReal)feedback.t2[0]; - t->y() = (sReal)feedback.t2[1]; - t->z() = (sReal)feedback.t2[2]; - } - - /** - * \brief reset the achor to the actual position. If a Node is moved or - * rotated by the editor, this function resets the constrains the joint - * applies to the Nodes that are connected. - * - * pre: - * - the joint should be created - * - * post: - * - */ - void JointPhysics::reattacheJoint(void) { - dReal pos[4] = {0,0,0,0}; - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointGetHingeAnchor(jointId, pos); - dJointSetHingeAnchor(jointId, pos[0], pos[1], pos[2]); - break; - case JOINT_TYPE_HINGE2: - dJointGetHinge2Anchor(jointId, pos); - dJointSetHinge2Anchor(jointId, pos[0], pos[1], pos[2]); - break; - case JOINT_TYPE_SLIDER: - // the slider joint has no ancher point - break; - case JOINT_TYPE_BALL: - dJointGetBallAnchor(jointId, pos); - dJointSetBallAnchor(jointId, pos[0], pos[1], pos[2]); - break; - case JOINT_TYPE_UNIVERSAL: - dJointGetUniversalAnchor(jointId, pos); - dJointSetUniversalAnchor(jointId, pos[0], pos[1], pos[2]); - break; - case JOINT_TYPE_FIXED: - dJointDestroy(jointId); - jointId = dJointCreateFixed(theWorld->getWorld(), 0); - dJointAttach(jointId, body1, body2); - dJointSetFixed(jointId); - dJointSetFeedback(jointId, &feedback); - // used for the integration study of the SpaceClimber - //dJointSetFixedParam(jointId, dParamCFM, cfm1);//0.0002); - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - } - - /** - * \brief Return the torque vector for the first axis of the joint - * - * v1[0] = b1_pos[0] - anchor[0]; - * v1[1] = b1_pos[1] - anchor[1]; - * v1[2] = b1_pos[2] - anchor[2]; - * radius = dSqrt(v1[0]*v1[0]+ - * v1[1]*v1[1]+ - * v1[2]*v1[2]); - * normal[0] = axis[1]*v1[2] - axis[2]*v1[1]; - * normal[1] = -axis[0]*v1[2] + axis[2]*v1[0]; - * normal[2] = axis[0]*v1[1] - axis[1]*v1[0]; - * dot = (normal[0]*feedback.f1[0]+ - * normal[1]*feedback.f1[1]+ - * normal[2]*feedback.f1[2]); - * normal[0] *= dot*radius; - * normal[1] *= dot*radius; - * normal[2] *= dot*radius; - */ - void JointPhysics::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 { - t->x() = axis2_torque.x(); - t->y() = axis2_torque.y(); - t->z() = axis2_torque.z(); - } - - /** - * \brief we need to calculate the axis torques and joint load before - * we can return it. - * - */ - void JointPhysics::update(void) { - const dReal *b1_pos, *b2_pos; - dReal anchor[4], axis[4], axis2[4]; - int calc1 = 0, calc2 = 0; - dReal radius, dot, torque; - dReal v1[3], normal[3], load[3], tmp1[3], axis_force[3]; - MutexLocker locker(&(theWorld->iMutex)); - - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointGetHingeAnchor(jointId, anchor); - dJointGetHingeAxis(jointId, axis); - calc1 = 1; - break; - case JOINT_TYPE_HINGE2: - dJointGetHinge2Anchor(jointId, anchor); - dJointGetHinge2Axis1(jointId, axis); - dJointGetHinge2Axis2(jointId, axis2); - calc1 = 1; - calc2 = 0; - break; - case JOINT_TYPE_SLIDER: - dJointGetSliderAxis(jointId, axis); - calc1 = 2; - break; - case JOINT_TYPE_BALL: - // no axis - break; - case JOINT_TYPE_UNIVERSAL: - dJointGetUniversalAnchor(jointId, anchor); - dJointGetUniversalAxis1(jointId, axis); - dJointGetUniversalAxis2(jointId, axis2); - calc1 = 1; - calc2 = 0; - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - motor_torque = feedback.lambda; - axis1_torque.x() = axis1_torque.y() = axis1_torque.z() = 0; - axis2_torque.x() = axis2_torque.y() = axis2_torque.z() = 0; - joint_load.x() = joint_load.y() = joint_load.z() = 0; - if(calc1 == 1) { - if(body1) { - b1_pos = dBodyGetPosition(body1); - dOP(v1, -, b1_pos, anchor); - //radius = dLENGTH(v1); - dCROSS(normal, =, axis, v1); - dot = dDOT(normal, feedback.f1); - dOPEC(normal, *=, dot); - dOP(load, -, feedback.f1, normal); - dCROSS(tmp1, =, v1, normal); - axis1_torque.x() = (sReal)tmp1[0]; - axis1_torque.y() = (sReal)tmp1[1]; - axis1_torque.z() = (sReal)tmp1[2]; - dCROSS(tmp1, =, v1, load); - joint_load.x() = (sReal)tmp1[0]; - joint_load.y() = (sReal)tmp1[1]; - joint_load.z() = (sReal)tmp1[2]; - // now nearly the same for the torque - dot = dDOT(axis, feedback.t1); - dOPC(tmp1, *, axis, dot); - dOP(load, -, feedback.t1, tmp1); - axis1_torque.x() += (sReal)tmp1[0]; - axis1_torque.y() += (sReal)tmp1[1]; - axis1_torque.z() += (sReal)tmp1[2]; - joint_load.x() += (sReal)load[0]; - joint_load.y() += (sReal)load[1]; - joint_load.z() += (sReal)load[2]; - } - else if(body2) { - // now we do it correct - // first get the position vector v1 - b2_pos = dBodyGetPosition(body2); - dOP(v1, -, b2_pos, anchor); - - // then differentiate the torque from the force - dot = dDOT(feedback.f2, v1) / dDOT(v1, v1); - dOPC(axis_force, *, v1, dot); - - // the difference is the torque - dOP(tmp1, -, feedback.f2, axis_force); - - // the torque value is given by: - torque = dLENGTH(tmp1) / dLENGTH(v1); - // then get the normal to v1 and the torque vector - dCROSS(normal, =, v1, tmp1); - // and scale the normal to represent the correct torque length - dOPEC(normal, *=, torque / dLENGTH(normal)); - // now the normal represents the torque vector at the anchor - // and we make a projection to the axis - dot = dDOT(normal, axis); - dOPC(tmp1, *, axis, dot); - dOP(load, -, normal, tmp1); - //dCROSS(tmp1, =, v1, normal); - axis1_torque.x() = (sReal)tmp1[0]; - axis1_torque.y() = (sReal)tmp1[1]; - axis1_torque.z() = (sReal)tmp1[2]; - //dCROSS(tmp1, =, v1, load); - joint_load.x() = (sReal)load[0]; - joint_load.y() = (sReal)load[1]; - joint_load.z() = (sReal)load[2]; - // now nearly the same for the torque - dot = dDOT(feedback.t2, axis); - dOPC(tmp1, *, axis, dot); - dOP(load, -, feedback.t2, tmp1); - //axis1_torque.x() += (sReal)tmp1[0]; - //axis1_torque.y() += (sReal)tmp1[1]; - //axis1_torque.z() += (sReal)tmp1[2]; - joint_load.x() += (sReal)load[0]; - joint_load.y() += (sReal)load[1]; - joint_load.z() += (sReal)load[2]; - } - } - else if(calc1 == 2) { - // this is for the slider - } - if(calc2 == 1) { - if(body1) { - b1_pos = dBodyGetPosition(body1); - dOP(v1, -, b1_pos, anchor); - radius = dLENGTH(v1); - dCROSS(normal, =, axis2, v1); - dot = dDOT(normal, feedback.f1); - dOPEC(normal, *=, dot*radius); - axis2_torque.x() = (sReal)normal[0]; - axis2_torque.y() = (sReal)normal[1]; - axis2_torque.z() = (sReal)normal[2]; - // now nearly the same for the torque - dot = dDOT(axis2, feedback.t1); - axis2_torque.x() += (sReal)(axis2[0]*dot); - axis2_torque.y() += (sReal)(axis2[1]*dot); - axis2_torque.z() += (sReal)(axis2[2]*dot); - } - if(body2) { - b2_pos = dBodyGetPosition(body2); - dOP(v1, -, b2_pos, anchor); - radius = dLENGTH(v1); - dCROSS(normal, =, axis2, v1); - dot = dDOT(normal, feedback.f2); - dOPEC(normal, *=, dot*radius); - axis2_torque.x() += (sReal)normal[0]; - axis2_torque.y() += (sReal)normal[1]; - axis2_torque.z() += (sReal)normal[2]; - // now nearly the same for the torque - dot = dDOT(axis2, feedback.t2); - axis2_torque.x() += (sReal)(axis2[0]*dot); - axis2_torque.y() += (sReal)(axis2[1]*dot); - axis2_torque.z() += (sReal)(axis2[2]*dot); - } - } - } - - void JointPhysics::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 { - MutexLocker locker(&(theWorld->iMutex)); - switch(joint_type) { - case JOINT_TYPE_HINGE: - return (sReal)dJointGetHingeAngleRate(jointId); - break; - case JOINT_TYPE_HINGE2: - return (sReal)dJointGetHinge2Angle1Rate(jointId); - break; - case JOINT_TYPE_SLIDER: - return (sReal)dJointGetSliderPositionRate(jointId); - break; - case JOINT_TYPE_BALL: - // no axis - break; - case JOINT_TYPE_UNIVERSAL: - return (sReal)dJointGetUniversalAngle1Rate(jointId); - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - return 0; - } - - sReal JointPhysics::getVelocity2(void) const { - MutexLocker locker(&(theWorld->iMutex)); - switch(joint_type) { - case JOINT_TYPE_HINGE: - break; - case JOINT_TYPE_HINGE2: - return (sReal)dJointGetHinge2Angle2Rate(jointId); - break; - case JOINT_TYPE_SLIDER: - break; - case JOINT_TYPE_BALL: - // no axis - break; - case JOINT_TYPE_UNIVERSAL: - return (sReal)dJointGetUniversalAngle2Rate(jointId); - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - return 0; - } - - void JointPhysics::setTorque(sReal torque) { - MutexLocker locker(&(theWorld->iMutex)); - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointAddHingeTorque(jointId, torque); - break; - case JOINT_TYPE_HINGE2: - break; - case JOINT_TYPE_SLIDER: - dJointAddSliderForce(jointId, torque); - break; - case JOINT_TYPE_BALL: - // no axis - break; - case JOINT_TYPE_UNIVERSAL: - - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - } - - void JointPhysics::setTorque2(sReal torque) { - CPP_UNUSED(torque); - switch(joint_type) { - case JOINT_TYPE_HINGE: - break; - case JOINT_TYPE_HINGE2: - break; - case JOINT_TYPE_SLIDER: - break; - case JOINT_TYPE_BALL: - // no axis - break; - case JOINT_TYPE_UNIVERSAL: - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - } - - void JointPhysics::changeStepSize(const JointData &jointS) { - MutexLocker locker(&(theWorld->iMutex)); - if(theWorld && theWorld->existsWorld()) { - calculateCfmErp(&jointS); - - switch(jointS.type) { - case JOINT_TYPE_HINGE: - 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); - } - else if(lo1 != 0) { - dJointSetHingeParam(jointId, dParamLoStop, lo1); - dJointSetHingeParam(jointId, dParamHiStop, hi1); - } - break; - case JOINT_TYPE_HINGE2: - 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); - } - break; - case JOINT_TYPE_SLIDER: - if(damping > 0.00000001) { - dJointSetSliderParam(jointId, dParamFMax, damping); - dJointSetSliderParam(jointId, dParamVel, 0); - } - if(spring > 0.00000001) { - dJointSetSliderParam(jointId, dParamLoStop, lo1); - dJointSetSliderParam(jointId, dParamHiStop, hi1); - dJointSetSliderParam(jointId, dParamStopCFM, cfm1); - dJointSetSliderParam(jointId, dParamStopERP, erp1); - } - else if(lo1 != 0) { - dJointSetSliderParam(jointId, dParamLoStop, lo1); - dJointSetSliderParam(jointId, dParamHiStop, hi1); - } - break; - case JOINT_TYPE_BALL: - /* - 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); - } - */ - break; - case JOINT_TYPE_UNIVERSAL: - 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); - } - break; - default: - // no correct type is spezified, so no physically node will be created - break; - } - } - } - - sReal JointPhysics::getMotorTorque(void) const { - return motor_torque; - } - - interfaces::sReal JointPhysics::getLowStop() const { - switch(joint_type) { - case JOINT_TYPE_HINGE: - return dJointGetHingeParam(jointId, dParamLoStop); - case JOINT_TYPE_HINGE2: - return dJointGetHinge2Param(jointId, dParamLoStop); - case JOINT_TYPE_SLIDER: - return dJointGetSliderParam(jointId, dParamLoStop); - default: - // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: lowstop for type %d not implemented yet\n", joint_type); - return 0.; - } - } - - interfaces::sReal JointPhysics::getHighStop() const { - switch(joint_type) { - case JOINT_TYPE_HINGE: - return dJointGetHingeParam(jointId, dParamHiStop); - case JOINT_TYPE_HINGE2: - return dJointGetHinge2Param(jointId, dParamHiStop); - case JOINT_TYPE_SLIDER: - return dJointGetSliderParam(jointId, dParamHiStop); - default: - // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: highstop for type %d not implemented yet\n", joint_type); - return 0.; - } - } - - interfaces::sReal JointPhysics::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); - return 0.; - } - } - - interfaces::sReal JointPhysics::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); - return 0.; - } - } - - interfaces::sReal JointPhysics::getCFM() const { - switch(joint_type) { - case JOINT_TYPE_HINGE: - return dJointGetHingeParam(jointId, dParamCFM); - case JOINT_TYPE_HINGE2: - return dJointGetHinge2Param(jointId, dParamCFM); - case JOINT_TYPE_SLIDER: - return dJointGetSliderParam(jointId, dParamCFM); - default: - // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: cfm for type %d not implemented yet\n", joint_type); - return 0.; - } - } - - void JointPhysics::setLowStop(interfaces::sReal lowStop) { - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointSetHingeParam(jointId, dParamLoStop, lowStop); - break; - case JOINT_TYPE_HINGE2: - dJointSetHinge2Param(jointId, dParamLoStop, lowStop); - break; - case JOINT_TYPE_SLIDER: - dJointSetSliderParam(jointId, dParamLoStop, lowStop); - break; - default: - // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: highstop for type %d not implemented yet\n", joint_type); - break; - } - } - - void JointPhysics::setHighStop(interfaces::sReal highStop) { - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointSetHingeParam(jointId, dParamHiStop, highStop); - break; - case JOINT_TYPE_HINGE2: - dJointSetHinge2Param(jointId, dParamHiStop, highStop); - break; - case JOINT_TYPE_SLIDER: - dJointSetSliderParam(jointId, dParamHiStop, highStop); - break; - default: - // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: highstop for type %d not implemented yet\n", joint_type); - break; - } - } - - void JointPhysics::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); - break; - } - } - - void JointPhysics::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); - break; - } - } - - void JointPhysics::setCFM(interfaces::sReal cfm) { - switch(joint_type) { - case JOINT_TYPE_HINGE: - dJointSetHingeParam(jointId, dParamCFM, cfm); - break; - case JOINT_TYPE_HINGE2: - dJointSetHinge2Param(jointId, dParamCFM, cfm); - break; - case JOINT_TYPE_SLIDER: - dJointSetSliderParam(jointId, dParamCFM, cfm); - break; - default: - // not implemented yes - fprintf(stderr, "mars::sim::JointPhysics: cfm for type %d not implemented yet\n", joint_type); - break; - } - } - - } // end of namespace sim -} // end of namespace mars diff --git a/sim/src/physics/JointPhysics.h b/sim/src/physics/JointPhysics.h deleted file mode 100644 index fb1dfd63b..000000000 --- a/sim/src/physics/JointPhysics.h +++ /dev/null @@ -1,124 +0,0 @@ -/* - * 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 . - * - */ - -#ifndef JOINT_PHYSICS_H -#define JOINT_PHYSICS_H - -#ifdef _PRINT_HEADER_ - #warning "JointPhysics.h" -#endif - -#include "WorldPhysics.h" - -#include -#include - -namespace mars { - namespace sim { - - class JointPhysics: public interfaces::JointInterface { - public: - ///the constructor - JointPhysics(std::shared_ptr world); - ///the destructor - virtual ~JointPhysics(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); - ///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 - virtual void setAnchor(const utils::Vector &anchor); - ///set the world informations - virtual void setAxis(const utils::Vector &axis); - virtual void setAxis2(const utils::Vector &axis); - virtual void getAxis(utils::Vector* axis) const; - virtual void getAxis2(utils::Vector* axis) const; - virtual void setWorldObject(std::shared_ptr world); - // methods to controll a joint through a motor - virtual void setForceLimit(interfaces::sReal max_force); - virtual void setForceLimit2(interfaces::sReal max_force); - virtual void setVelocity(interfaces::sReal velocity); - virtual void setVelocity2(interfaces::sReal velocity); - virtual interfaces::sReal getVelocity(void) const; - virtual interfaces::sReal getVelocity2(void) const; - virtual void setJointAsMotor(int axis); - virtual void unsetJointAsMotor(int axis); - virtual interfaces::sReal getPosition(void) const; - virtual interfaces::sReal getPosition2(void) const; - virtual void getForce1(utils::Vector *f) const; - virtual void getForce2(utils::Vector *f) const; - virtual void getTorque1(utils::Vector *t) const; - virtual void getTorque2(utils::Vector *t) const; - virtual void setTorque(interfaces::sReal torque); - virtual void setTorque2(interfaces::sReal torque); - virtual void reattacheJoint(void); - virtual void update(void); - virtual void getJointLoad(utils::Vector *t) const; - virtual void getAxisTorque(utils::Vector *t) const; - virtual void getAxis2Torque(utils::Vector *t) const; - virtual void changeStepSize(const interfaces::JointData &jointS); - virtual interfaces::sReal getMotorTorque(void) const; - virtual interfaces::sReal getLowStop() const; - virtual interfaces::sReal getHighStop() const; - virtual interfaces::sReal getLowStop2() const; - virtual interfaces::sReal getHighStop2() const; - virtual interfaces::sReal getCFM() const; - virtual void setLowStop(interfaces::sReal lowStop); - virtual void setHighStop(interfaces::sReal highStop); - virtual void setLowStop2(interfaces::sReal lowStop2); - virtual void setHighStop2(interfaces::sReal highStop2); - virtual void setCFM(interfaces::sReal cfm); - - private: - std::shared_ptr theWorld; - dJointID jointId, ball_motor; - dJointFeedback feedback; - dBodyID body1, body2; - int joint_type; - dReal cfm, cfm1, cfm2, erp1, erp2; - dReal lo1, lo2, hi1, hi2; - 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); - }; - - } // end of namespace sim -} // end of namespace mars - -#endif diff --git a/sim/src/physics/NodePhysics.cpp b/sim/src/physics/NodePhysics.cpp deleted file mode 100644 index 02a7c988c..000000000 --- a/sim/src/physics/NodePhysics.cpp +++ /dev/null @@ -1,1879 +0,0 @@ -/* - * 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 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" -#include "ODEObjectFactory.h" -#include "ODEBoxCreater.h" -#include "../sensors/RotatingRaySensor.h" - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - - -namespace mars { - namespace sim { - - using namespace utils; - using namespace interfaces; - - /** - * \brief Creates a empty node objekt. - * - * \pre - * - the pointer to the physics Interface should be correct. - * This implementation can be a bad trap. The class that implements the - * physics interface, have to inherit from the interface at first, - * otherwise this implementation will cause bad error while pointing - * an incorrect adresses. - * - * \post - * - 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. ^_^ - 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; - dMassSetZero(&nMass); - } - - /** - * \brief Destroys the node in the physical world. - * - * pre: - * - theWorld is the correct world object - * - * post: - * - all physical representation of the node should be cleared - * - * are the geom and the body realy all thing to take care of? - */ - NodePhysics::~NodePhysics(void) { - std::vector::iterator iter; - MutexLocker locker(&(theWorld->iMutex)); - - if(nBody) theWorld->destroyBody(nBody, this); - - 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){ - delete ((*iter).gd); - (*iter).gd = 0; - } - 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); - } - - /** - * \brief The method creates an ode node, which properties are given by - * the NodeData param node. - * - * 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 - * - * post: - * - a physical node should be created in the world - * - the node properties should be set - */ - bool NodePhysics::createNode(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; - //LOG_DEBUG("physicMode %d", node->physicMode); - // 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 - return 0; - break; - } - if(ret == 0) { - // Error createing the physical Node - return 0; - } - - // 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) { - 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) { - dGeomGetQuaternion(nGeom, t1); - dQMultiply0(t2, tmp, t1); - dNormalize4(t2); - dGeomSetQuaternion(nGeom, t2); - } - else - dGeomSetQuaternion(nGeom, tmp); - } - node_data.id = node->index; - if(node->map.hasKey("c_filter_depth")) { - node_data.filter_depth = node->map["c_filter_depth"]; - } - if(node->map.hasKey("c_filter_angle")) { - node_data.filter_angle = node->map["c_filter_angle"]; - } - if(node->map.hasKey("c_filter_sphere")) { - node_data.filter_sphere.x() = node->map["c_filter_sphere"][0]; - node_data.filter_sphere.y() = node->map["c_filter_sphere"][1]; - node_data.filter_sphere.z() = 0.0;//node->map["c_filter_sphere"][2]; - node_data.filter_radius = node->map["c_filter_sphere"][3]; - } - dGeomSetData(nGeom, &node_data); - locker.unlock(); - setContactParams(node->c_params); - return 1; - } - LOG_WARN("NodePhysics: tried to create a Node 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 { - MutexLocker locker(&(theWorld->iMutex)); - if(nBody) { - const dReal* tmp = dGeomGetPosition(nGeom); - pos->x() = (sReal)tmp[0]; - pos->y() = (sReal)tmp[1]; - pos->z() = (sReal)tmp[2]; - /* - if(composite) { - tmp = dGeomGetOffsetPosition(nGeom); - pos->x += (sReal)tmp[0]; - pos->y += (sReal)tmp[1]; - pos->z += (sReal)tmp[2]; - }*/ - } - else if(nGeom) { - const dReal* tmp = dGeomGetPosition(nGeom); - pos->x() = (sReal)tmp[0]; - pos->y() = (sReal)tmp[1]; - pos->z() = (sReal)tmp[2]; - } - else { - pos->x() = 0; - pos->y() = 0; - pos->z() = 0; - } - } - - /** - * \brief The method sets the position of the physical node model to the - * position of the param. If move_group is set, all nodes of a composite - * group will be moved, otherwise only this node will be moved. A vector - * from the old position to the new will be returned. - * - * I don't know if we should use this function in a way like it is - * implemented now. The pre and post conditions could loke like this: - * - * pre: - * - there should be a physical representation of the node - * - the pos param should point to a correct position struct - * - * post: - * - if there is a physically representation and the node is movable - * 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 dReal *tpos; - const dReal *tpos2; - dReal npos[3]; - Vector offset; - MutexLocker locker(&(theWorld->iMutex)); - - if(composite) { - if(move_group) { - /* - brot = dBodyGetQuaternion(nBody); - tpos2 = dGeomGetOffsetPosition(nGeom); - tpos = dBodyGetPosition(nBody); - dQtoR(brot, R); - dMULTIPLY0_331(npos, R, tpos2); - offset.x = pos->x - (sReal)(tpos[0]+npos[0]); - offset.y = pos->y - (sReal)(tpos[1]+npos[1]); - offset.z = pos->z - (sReal)(tpos[2]+npos[2]); - */ - tpos2 = dGeomGetPosition(nGeom); - offset.x() = pos.x() - (sReal)(tpos2[0]); - offset.y() = pos.y() - (sReal)(tpos2[1]); - offset.z() = pos.z() - (sReal)(tpos2[2]); - tpos = dBodyGetPosition(nBody); - dBodySetPosition(nBody, tpos[0] + (dReal)offset.x(), - tpos[1] + (dReal)offset.y(), - tpos[2] + (dReal)offset.z()); - return offset; - } - else { - tpos = dBodyGetPosition(nBody); - tpos2 = dGeomGetOffsetPosition(nGeom); - npos[0] = tpos[0] + tpos2[0]; - npos[1] = tpos[1] + tpos2[1]; - npos[2] = tpos[2] + tpos2[2]; - offset.x() = pos.x() - (sReal)(npos[0]); - offset.y() = pos.y() - (sReal)(npos[1]); - offset.z() = pos.z() - (sReal)(npos[2]); - //std::cout << " " << pos.z(); - dGeomSetOffsetWorldPosition(nGeom, (dReal)pos.x(), (dReal)pos.y(), - (dReal)pos.z()); - // here we have to recalculate the mass - theWorld->resetCompositeMass(nBody); - return offset; - } - } - else { - if(nBody) { - tpos = dBodyGetPosition(nBody); - offset.x() = pos.x() - (sReal)(tpos[0]); - offset.y() = pos.y() - (sReal)(tpos[1]); - offset.z() = pos.z() - (sReal)(tpos[2]); - dBodySetPosition(nBody, (dReal)pos.x(), (dReal)pos.y(), (dReal)pos.z()); - return offset; - } - else if(nGeom) { - //tpos = dGeomGetPosition(nGeom); - //offset.x() = pos->x - (sReal)(tpos[0]); - //offset.y() = pos->y - (sReal)(tpos[1]); - //offset.z() = pos->z - (sReal)(tpos[2]); - dGeomSetPosition(nGeom, (dReal)pos.x(), (dReal)pos.y(), (dReal)pos.z()); - return offset; - } - } - return offset; - } - - /** - * \brief The method copies the Quaternion of the physically node at the - * adress of the Quaternion pointer q. - * - * pre: - * - there should be a physical representation of the node - * - the node should be movable - * - the q param should point to a correct Quaternion struct - * - * post: - * - if there is a physical representation and the node is movable - * the Quaternion struct should be filled with the physical rotation - * of the node - * - otherwise a standard return of zero rotation should be set - */ - void NodePhysics::getRotation(Quaternion* q) const { - dQuaternion tmp; - MutexLocker locker(&(theWorld->iMutex)); - - if(nBody || nGeom) { - dGeomGetQuaternion(nGeom, tmp); - q->x() = (sReal)tmp[1]; - q->y() = (sReal)tmp[2]; - q->z() = (sReal)tmp[3]; - q->w() = (sReal)tmp[0]; - } - else { - q->x() = (sReal)0; - q->y() = (sReal)0; - q->z() = (sReal)0; - q->w() = (sReal)1; - } - } - - /** - * \brief The method copies the linear velocity of the physically node at the - * adress of the linear_vel pointer vel. - * - * pre: - * - there should be a physical representation of the node - * - the node should be movable - * - the vel param should point to a correct linear_vel struct - * - * post: - * - if there is a physical representation and the node is movable - * the linear_vel struct should be filled with the physical linar - * velocity of the node - * - otherwise a standard return of zero velocity should be set - */ - void NodePhysics::getLinearVelocity(Vector* vel) const { - const dReal *tmp; - MutexLocker locker(&(theWorld->iMutex)); - - if(nBody) { - tmp = dBodyGetLinearVel(nBody); - vel->x() = (sReal)tmp[0]; - vel->y() = (sReal)tmp[1]; - vel->z() = (sReal)tmp[2]; - } - else { - vel->x() = (sReal)0; - vel->y() = (sReal)0; - vel->z() = (sReal)0; - } - } - - /** - * \brief The method copies the angular velocity of the physically node at the - * adress of the angular_vel pointer vel. - * - * pre: - * - there should be a physical representation of the node - * - the node should be movable - * - the vel param should point to a correct angular_vel struct - * - * post: - * - if there is a physical representation and the node is movable - * the angular_vel struct should be filled with the physical angular - * velocity of the node - * - otherwise a standard return of zero velocity should be set - */ - void NodePhysics::getAngularVelocity(Vector* vel) const { - const dReal *tmp; - MutexLocker locker(&(theWorld->iMutex)); - - if(nBody) { - tmp = dBodyGetAngularVel(nBody); - vel->x() = (sReal)tmp[0]; - vel->y() = (sReal)tmp[1]; - vel->z() = (sReal)tmp[2]; - } - else { - vel->x() = (sReal)0; - vel->y() = (sReal)0; - vel->z() = (sReal)0; - } - } - - /** - * \brief The method copies the force of the physically node at the - * adress of the force pointer force. - * - * pre: - * - there should be a physical representation of the node - * - the node should be movable - * - the f param should point to a correct force struct - * - * post: - * - if there is a physical representation and the node is movable - * the force struct should be filled with the physical - * force of the node - * - otherwise a standard return of zero force should be set - */ - void NodePhysics::getForce(Vector* f) const { - const dReal *tmp; - MutexLocker locker(&(theWorld->iMutex)); - - if(nBody) { - tmp = dBodyGetForce(nBody); - f->x() = (sReal)tmp[0]; - f->y() = (sReal)tmp[1]; - f->z() = (sReal)tmp[2]; - } - else { - f->x() = (sReal)0; - f->y() = (sReal)0; - f->z() = (sReal)0; - } - } - - /** - * \brief The method copies the torque of the physically node at the - * adress of the torque pointer force. - * - * pre: - * - there should be a physical representation of the node - * - the node should be movable - * - the t param should point to a correct torque struct - * - * post: - * - if there is a physical representation and the node is movable - * the torque struct should be filled with the physical torque - * of the node - * - otherwise a standard return of zero torque should be set - */ - void NodePhysics::getTorque(Vector *t) const { - const dReal *tmp; - MutexLocker locker(&(theWorld->iMutex)); - - if(nBody) { - tmp = dBodyGetTorque(nBody); - t->x() = (sReal)tmp[0]; - t->y() = (sReal)tmp[1]; - t->z() = (sReal)tmp[2]; - } - else { - t->x() = (sReal)0; - t->y() = (sReal)0; - t->z() = (sReal)0; - } - } - - - /** - * \brief This method sets the rotation of the physically node. - * - * I don't if and how to use this function yet. ^-^ - * 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) { - dQuaternion tmp, tmp2, tmp3, tmp4, tmp5; - const dReal *brot, *bpos, *gpos; - Quaternion q2; - dMatrix3 R; - dVector3 pos, new_pos, new2_pos; - MutexLocker locker(&(theWorld->iMutex)); - - pos[0] = pos[1] = pos[2] = 0; - tmp[1] = (dReal)q.x(); - tmp[2] = (dReal)q.y(); - tmp[3] = (dReal)q.z(); - tmp[0] = (dReal)q.w(); - if(nBody) { - bpos = dBodyGetPosition(nBody); - pos[0] = bpos[0]; - pos[1] = bpos[1]; - pos[2] = bpos[2]; - brot = dBodyGetQuaternion(nBody); - tmp4[0] = brot[0]; - tmp4[1] = brot[1]; - tmp4[2] = brot[2]; - tmp4[3] = brot[3]; - if(composite && !move_group) { - dGeomGetQuaternion(nGeom, tmp2); - dGeomSetOffsetWorldQuaternion(nGeom, tmp); - } - else if (composite) { - dGeomGetQuaternion(nGeom, tmp2); - dGeomGetOffsetQuaternion(nGeom, tmp3); - tmp3[1] *= -1; - tmp3[2] *= -1; - tmp3[3] *= -1; - dQMultiply0(tmp5, tmp, tmp3); - dBodySetQuaternion(nBody, tmp5); - } - else { - tmp2[0] = brot[0]; - tmp2[1] = brot[1]; - tmp2[2] = brot[2]; - tmp2[3] = brot[3]; - dBodySetQuaternion(nBody, tmp); - } - } - else if(nGeom) { - dGeomGetQuaternion(nGeom, tmp2); - dGeomSetQuaternion(nGeom, tmp); - } - dQMultiply2(tmp3, tmp, tmp2); - q2.x() = (sReal)tmp3[1]; - q2.y() = (sReal)tmp3[2]; - q2.z() = (sReal)tmp3[3]; - q2.w() = (sReal)tmp3[0]; - if(nBody && composite && move_group) { - gpos = dGeomGetOffsetPosition(nGeom); - dQtoR(tmp4, R); - dMULTIPLY0_331(new_pos, R, gpos); - pos[0] += new_pos[0]; - pos[1] += new_pos[1]; - pos[2] += new_pos[2]; - new_pos[0] *= -1; - new_pos[1] *= -1; - new_pos[2] *= -1; - dQtoR(tmp3, R); - dMULTIPLY0_331(new2_pos, R, new_pos); - new_pos[0] = pos[0]+new2_pos[0]; - new_pos[1] = pos[1]+new2_pos[1]; - new_pos[2] = pos[2]+new2_pos[2]; - dBodySetPosition(nBody, new_pos[0], new_pos[1], new_pos[2]); - } - return q2; - } - - /** - * \brief This function sets the pointer to the physical world object. - * - * I don't think that we need this function. - */ - void NodePhysics::setWorldObject(std::shared_ptr world) { - theWorld = std::dynamic_pointer_cast(world); - } - - /** - *\brief return the body; - * this function is created to make it possible to get the - * body from joint physics - *TO DO : test if the Node has a body - */ - dBodyID NodePhysics::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) { - bool body_created = 1; - dQuaternion tmp; - - tmp[1] = (dReal)node->rot.x(); - tmp[2] = (dReal)node->rot.y(); - tmp[3] = (dReal)node->rot.z(); - tmp[0] = (dReal)node->rot.w(); - - // first we must find or create a physically body for the node - // and connect the geometry to the body - if(node->groupID) { - body_created = theWorld->getCompositeBody(node->groupID, &nBody, this); - composite = true; - } - else { - if(!nBody) nBody = dBodyCreate(theWorld->getWorld()); - } - //dBodySetLinearDamping(nBody, 0.5); - //dBodySetAngularDamping(nBody, 0.5); - dGeomSetBody(nGeom, nBody); - - // if a new body was created, we have to set the initial position - // and rotation of the body, otherwise have to set the position - // and rotation of the geom - if(body_created) { - dBodySetMass(nBody, &nMass); - dBodySetPosition(nBody, (dReal)(node->pos.x()), - (dReal)(node->pos.y()), - (dReal)(node->pos.z())); - // ## the rotation has to be defined as quanternion or matrix? ## - dBodySetQuaternion(nBody, tmp); - } - else if(node->density > 0 || node->mass > 0) { - dMass bodyMass; - - dBodyGetMass(nBody, &bodyMass); - dGeomSetOffsetWorldPosition(nGeom, (dReal)(node->pos.x()), - (dReal)(node->pos.y()), - (dReal)(node->pos.z())); - // ## the same for the rotation here ## - dGeomSetOffsetWorldQuaternion(nGeom, tmp); - addMassToCompositeBody(nBody, &bodyMass); - dBodySetMass(nBody, &bodyMass); - } -#ifdef _DEBUG_MASS_ - fprintf(stderr, "%mass id: %d %g\n", node->index, nMass.mass); - fprintf(stderr, "\t%g\t%g\t%g\n", nMass.I[0], nMass.I[1], nMass.I[2]); - fprintf(stderr, "\t%g\t%g\t%g\n", nMass.I[4], nMass.I[5], nMass.I[6]); - fprintf(stderr, "\t%g\t%g\t%g\n", nMass.I[8], nMass.I[9], nMass.I[10]); -#endif - } - - /** - * \brief executes an rotation at a given point and returns the - * new position of the node - * - * pre: - * - * post: - */ - const Vector NodePhysics::rotateAtPoint(const Vector &rotation_point, - const Quaternion &rotation, - bool move_group) { - dQuaternion tmp, tmp2, tmp3; - const dReal *bpos, *gpos, *brot; - dVector3 pos, new_pos; - Vector npos; - dMatrix3 R; - MutexLocker locker(&(theWorld->iMutex)); - - tmp[1] = (dReal)rotation.x(); - tmp[2] = (dReal)rotation.y(); - tmp[3] = (dReal)rotation.z(); - tmp[0] = (dReal)rotation.w(); - if(composite) { - brot = dBodyGetQuaternion(nBody); - dQMultiply0(tmp2, tmp, brot); - if(move_group) { - // we have to rotate the body and return the new position of the geom - dBodySetQuaternion(nBody, tmp2); - dQtoR(tmp, R); - bpos = dBodyGetPosition(nBody); - pos[0] = bpos[0] - (dReal)rotation_point.x(); - pos[1] = bpos[1] - (dReal)rotation_point.y(); - pos[2] = bpos[2] - (dReal)rotation_point.z(); - dMULTIPLY0_331(new_pos, R, pos); - npos.x() = new_pos[0] + (dReal)rotation_point.x(); - npos.y() = new_pos[1] + (dReal)rotation_point.y(); - npos.z() = new_pos[2] + (dReal)rotation_point.z(); - dBodySetPosition(nBody, (dReal)npos.x(), (dReal)npos.y(), (dReal)npos.z()); - - gpos = dGeomGetOffsetPosition(nGeom); - npos.x() += (sReal)(gpos[0]); - npos.y() += (sReal)(gpos[1]); - npos.z() += (sReal)(gpos[2]); - return npos; - } - else { - dGeomGetQuaternion(nGeom, tmp3); - dQMultiply0(tmp2, tmp, tmp3); - dNormalize4(tmp2); - dGeomSetOffsetWorldQuaternion(nGeom, tmp2); - - gpos = dGeomGetPosition(nGeom); - pos[0] = gpos[0] - (dReal)rotation_point.x(); - pos[1] = gpos[1] - (dReal)rotation_point.y(); - pos[2] = gpos[2] - (dReal)rotation_point.z(); - dQtoR(tmp, R); - dMULTIPLY0_331(new_pos, R, pos); - pos[0] = new_pos[0] + (dReal)rotation_point.x(); - pos[1] = new_pos[1] + (dReal)rotation_point.y(); - pos[2] = new_pos[2] + (dReal)rotation_point.z(); - dGeomSetOffsetWorldPosition(nGeom, pos[0], pos[1], pos[2]); - npos.x() = (sReal)(pos[0]); - npos.y() = (sReal)(pos[1]); - npos.z() = (sReal)(pos[2]); - return npos; - } - } - // the last two cases do in principle the same - // we have to rotate the geom and calculate the translation - /* - else if(nBody) { - - }*/ - else if(nGeom) { - dGeomGetQuaternion(nGeom, tmp2); - dQMultiply0(tmp3, tmp, tmp2); - dGeomSetQuaternion(nGeom, tmp3); - dQtoR(tmp, R); - gpos = dGeomGetPosition(nGeom); - pos[0] = gpos[0] - (dReal)rotation_point.x(); - pos[1] = gpos[1] - (dReal)rotation_point.y(); - pos[2] = gpos[2] - (dReal)rotation_point.z(); - dMULTIPLY0_331(new_pos, R, pos); - npos.x() = new_pos[0] + (dReal)rotation_point.x(); - npos.y() = new_pos[1] + (dReal)rotation_point.y(); - npos.z() = new_pos[2] + (dReal)rotation_point.z(); - dGeomSetPosition(nGeom, (dReal)npos.x(), (dReal)npos.y(), (dReal)npos.z()); - return npos; - } - 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 - * - * - * pre: - * - * post: - */ - dMass NodePhysics::getODEMass(void) const { - return nMass; - } - - /** - * \brief Sets the linear velocity of a node - * - * pre: - * - the node should have a body - * - * post: - * - the linear velocity of the body should be set - */ - void NodePhysics::setLinearVelocity(const Vector &velocity) { - MutexLocker locker(&(theWorld->iMutex)); - if(nBody) dBodySetLinearVel(nBody, (dReal)velocity.x(), - (dReal)velocity.y(), (dReal)velocity.z()); - } - - /** - * \brief Sets the angular velocity of a node - * - * pre: - * - the node should have a body - * - * post: - * - the angular velocity of the body should be set - */ - void NodePhysics::setAngularVelocity(const Vector &velocity) { - MutexLocker locker(&(theWorld->iMutex)); - if(nBody) dBodySetAngularVel(nBody, (dReal)velocity.x(), - (dReal)velocity.y(), (dReal)velocity.z()); - } - - /** - * \brief Sets the force of a node - * - * pre: - * - the node should have a body - * - * post: - * - the force of the body should be set - */ - void NodePhysics::setForce(const Vector &f) { - MutexLocker locker(&(theWorld->iMutex)); - if(nBody) dBodySetForce(nBody, (dReal)f.x(), - (dReal)f.y(), (dReal)f.z()); - } - - /** - * \brief Sets the torque of a node - * - * pre: - * - the node should have a body - * - * post: - * - the torque of the body should be set - */ - void NodePhysics::setTorque(const Vector &t) { - MutexLocker locker(&(theWorld->iMutex)); - if(nBody) dBodySetTorque(nBody, (dReal)t.x(), - (dReal)t.y(), (dReal)t.z()); - } - - /** - * \brief Adds a off-center force to a node - * - * pre: - * - the node should have a body - * - * post: - * - the force should be added to the body - */ - void NodePhysics::addForce(const Vector &f, const Vector &p) { - MutexLocker locker(&(theWorld->iMutex)); - if(nBody) { - dBodyAddForceAtPos(nBody, - (dReal)f.x(), (dReal)f.y(), (dReal)f.z(), - (dReal)p.x(), (dReal)p.y(), (dReal)p.z()); - } - } - /** - * \brief Adds a force to a node - * - * pre: - * - the node should have a body - * - * post: - * - the force should be added to the body - */ - void NodePhysics::addForce(const Vector &f) { - MutexLocker locker(&(theWorld->iMutex)); - if(nBody) { - dBodyAddForce(nBody, (dReal)f.x(), (dReal)f.y(), (dReal)f.z()); - } - } - - /** - * \brief Adds a torque to a node - * - * pre: - * - the node should have a body - * - * post: - * - the torque should be added to the body - */ - void NodePhysics::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 { - if(nGeom) { - return node_data.num_ground_collisions; - } - return false; - } - - void NodePhysics::getContactPoints(std::vector *contact_points) const { - contact_points->clear(); - if(nGeom) { - std::vector::const_iterator iter; - - for(iter=node_data.contact_points.begin(); - iter!=node_data.contact_points.end(); ++iter) - - contact_points->push_back((*iter)); - } - } - - void NodePhysics::getContactIDs(std::list *ids) const { - ids->clear(); - if(nGeom) { - *ids = node_data.contact_ids; - } - } - - - sReal NodePhysics::getGroundContactForce(void) const { - std::vector::const_iterator iter; - dReal force[3] = {0,0,0}; - - if(nGeom) { - for(iter = node_data.ground_feedbacks.begin(); - iter != node_data.ground_feedbacks.end(); iter++) { - if(node_data.node1) { - force[0] += (*iter)->f1[0]; - force[1] += (*iter)->f1[1]; - force[2] += (*iter)->f1[2]; - } - else { - force[0] += (*iter)->f2[0]; - force[1] += (*iter)->f2[1]; - force[2] += (*iter)->f2[2]; - } - } - } - return dLENGTH(force); - } - - const Vector NodePhysics::getContactForce(void) const { - std::vector::const_iterator iter; - dReal force[3] = {0,0,0}; - - if(nGeom) { - for(iter = node_data.ground_feedbacks.begin(); - iter != node_data.ground_feedbacks.end(); iter++) { - if(node_data.node1) { - force[0] += (*iter)->f1[0]; - force[1] += (*iter)->f1[1]; - force[2] += (*iter)->f1[2]; - } - else { - force[0] += (*iter)->f2[0]; - force[1] += (*iter)->f2[1]; - force[2] += (*iter)->f2[2]; - } - } - } - return Vector(force[0], force[1], force[2]); - } - - void NodePhysics::addCompositeOffset(dReal x, dReal y, dReal z) { - // no lock because physics internal functions get locked elsewhere - const dReal *gpos; - - gpos = dGeomGetPosition(nGeom); - dGeomSetOffsetWorldPosition(nGeom, gpos[0]+x, gpos[1]+y, gpos[2]+z); - } - - void NodePhysics::addMassToCompositeBody(dBodyID theBody, dMass *bodyMass) { - // no lock because physics internal functions get locked elsewhere - const dReal *rotation, *pos, *brot; - dReal tpos[3]; - dMatrix3 R; - dMass myMass = nMass; - - // first rotate and translate the mass and add it to the body - // we get the rotation and translation relative to the body-frame - // from the geom - rotation = dGeomGetOffsetRotation(nGeom); - pos = dGeomGetOffsetPosition(nGeom); - dMassRotate(&myMass, rotation); - dMassTranslate(&myMass, pos[0], pos[1], pos[2]); - dMassAdd(bodyMass, &myMass); - // the mass displacement is in bodyframe - // to get the displacement in world koordinates - // we have to rotate the vector - brot = dBodyGetQuaternion(theBody); - dRfromQ(R, brot); - dMULTIPLY0_331(tpos, R, bodyMass->c); - // then we can add the vector to the geoms and body - theWorld->moveCompositeMassCenter(theBody, tpos[0], tpos[1], tpos[2]); - dMassTranslate(bodyMass, -bodyMass->c[0], -bodyMass->c[1], -bodyMass->c[2]); - } - - void NodePhysics::getAbsMass(dMass *tMass) const { - // no lock because physics internal functions get locked elsewhere - const dReal *pos = dGeomGetPosition(nGeom); - const dReal *rot = dGeomGetRotation(nGeom); - - *tMass = nMass; - dMassRotate(tMass, rot); - 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) { - MutexLocker locker(&(theWorld->iMutex)); - node_data.c_params = c_params; - if(nGeom) { - dGeomSetCollideBits(nGeom, c_params.coll_bitmask); - dGeomSetCategoryBits(nGeom, c_params.coll_bitmask); - } - } - - /** - * \brief This function adds a new sensor to the physical node. - * Sensors that are implemented are a "ray sensor" and a "multi ray sensor" - * - * - * pre: - * - the BaseSensor should contain a complete and correct configuration - * - the world have to exists because geoms have to be created - * - this Node should have an existing geom - * - * post: - * - every new sensor has to be added to a list - * - 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) { - MutexLocker locker(&(theWorld->iMutex)); - int i; - geom_data* gd; - sensor_list_element sle; - const dReal *pos = dGeomGetPosition(nGeom); - const dReal *rot = dGeomGetRotation(nGeom); - Vector direction; - dVector3 dest, tmp; - //sReal rad_angle, rad_steps, rad_start; - double rad_steps, rad_start; - - - BasePolarIntersectionSensor *polarSensor; - polarSensor = dynamic_cast(sensor); - - //case SENSOR_TYPE_RAY: - if(polarSensor){ - sle.sensor = sensor; - sle.updateTime = 0.0; - //sensor.count_data = sensor.resolution; - //sensor.data = (sReal*)malloc(sensor.resolution * sizeof(sReal)); - - mars::sim::RotatingRaySensor* rotRaySensor = dynamic_cast(sensor); - if(rotRaySensor){ - int N = rotRaySensor->getNumberRays(); - std::vector& directions = rotRaySensor->getDirections(); - assert(N == directions.size()); - - // Requests and adds the single rays using the local sensor frame. - for(i=0; isense_contact_force = 0; - (*polarSensor)[i] = gd->value = polarSensor->maxDistance;//sensor.max_distance; - gd->ray_sensor = 1; - gd->parent_geom = nGeom; - gd->parent_body = nBody; - sle.geom = dCreateRay(NULL, polarSensor->maxDistance); - dGeomRaySetClosestHit(sle.geom, 1); - dGeomSetCollideBits(sle.geom, 32768); - dGeomSetCategoryBits(sle.geom, 32768); - - // Use the precalculated ray directions of the sensor. - direction = /*polarSensor->getOrientation() **/ directions[i]; - sle.ray_direction = direction; - tmp[0] = direction.x(); - tmp[1] = direction.y(); - tmp[2] = direction.z(); - dMULTIPLY0_331(dest, rot, tmp); - dGeomRaySet(sle.geom, pos[0], pos[1], pos[2], dest[0], dest[1], dest[2]); - sle.gd = gd; - sle.index = i; - sensor_list.push_back(sle); - dGeomSetData(sle.geom, gd); - //dGeomRaySetParams(sle.geom, 1, 1); - dGeomSetCollideBits(sle.geom, COLLIDE_MASK_SENSOR); - dGeomSetCategoryBits(sle.geom, COLLIDE_MASK_SENSOR); - dGeomDisable(sle.geom); - } - } else { - //rad_angle = polarSensor->widthX*; //M_PI*sensor.flare_angle/180; - rad_steps = polarSensor->getCols(); //rad_angle/(sReal)(sensor.resolution-1); - rad_start = -((rad_steps-1)/2.0)*polarSensor->stepX; //Starting to Left, because 0 is in front and rock convention posive CCW //(M_PI-rad_angle)/2; - if(rad_steps == 1){ - rad_start = 0; - } - for(i=0; isense_contact_force = 0; - (*polarSensor)[i] = gd->value = polarSensor->maxDistance;//sensor.max_distance; - - gd->ray_sensor = 1; - gd->parent_geom = nGeom; - gd->parent_body = nBody; - sle.geom = dCreateRay(NULL, polarSensor->maxDistance); - dGeomRaySetClosestHit(sle.geom, 1); - dGeomSetCollideBits(sle.geom, 32768); - dGeomSetCategoryBits(sle.geom, 32768); - direction = Vector(cos(rad_start+i*polarSensor->stepX), - sin(rad_start+i*polarSensor->stepX), 0); - //direction = QVRotate(sensor.rotation, direction); - direction = (polarSensor->getOrientation() * direction); - sle.ray_direction = direction; - tmp[0] = direction.x(); - tmp[1] = direction.y(); - tmp[2] = direction.z(); - dMULTIPLY0_331(dest, rot, tmp); - dGeomRaySet(sle.geom, pos[0], pos[1], pos[2], dest[0], dest[1], dest[2]); - sle.gd = gd; - sle.index = i; - sensor_list.push_back(sle); - dGeomSetData(sle.geom, gd); - //dGeomRaySetParams(sle.geom, 1, 1); - dGeomSetCollideBits(sle.geom, COLLIDE_MASK_SENSOR); - dGeomSetCategoryBits(sle.geom, COLLIDE_MASK_SENSOR); - dGeomDisable(sle.geom); - } - } - } - - BaseGridIntersectionSensor *polarGridSensor; - polarGridSensor = dynamic_cast(sensor); - - if(polarGridSensor){ - sle.sensor = sensor; - sle.updateTime = 0.0; - int cols, rows; - dVector3 dir={0,0,0,0}, xStep={0,0,0,0}, - yStep={0,0,0,0}, xOffset={0,0,0,0}, yOffset={0,0,0,0}; - - cols = polarGridSensor->getCols(); - rows = polarGridSensor->getRows(); - - tmp[0] = 0; - tmp[1] = 0; - tmp[2] = -1; - dMULTIPLY0_331(dir, rot, tmp); - tmp[0] = polarGridSensor->stepX; - tmp[1] = 0; - tmp[2] = 0; - //dMULTIPLY0_331(xStep, rot, tmp); - tmp[0] = 0; - tmp[1] = polarGridSensor->stepY; - tmp[2] = 0; - //dMULTIPLY0_331(yStep, rot, tmp); - - - for(int x=0; xsense_contact_force = 0; - (*polarGridSensor)[y*cols+x] = gd->value = polarGridSensor->maxDistance; - - gd->ray_sensor = 1; - gd->parent_geom = nGeom; - sle.geom = dCreateRay(theWorld->getSpace(), - polarGridSensor->maxDistance); - dGeomRaySetClosestHit(sle.geom, 1); - dGeomSetCollideBits(sle.geom, 32768); - dGeomSetCategoryBits(sle.geom, 32768); - - direction = (polarGridSensor->getOrientation() * - Vector(0.0, 0.0, -1.0)); - sle.ray_direction = direction; - tmp[0] = direction.x(); - tmp[1] = direction.y(); - tmp[2] = direction.z(); - dMULTIPLY0_331(dest, rot, tmp); - xOffset[0] = -cols*0.5*xStep[0] + x*xStep[0]; - xOffset[1] = -cols*0.5*xStep[1] + x*xStep[1]; - xOffset[2] = -cols*0.5*xStep[2] + x*xStep[2]; - - yOffset[0] = -rows*0.5*yStep[0] + y*yStep[0]; - yOffset[1] = -rows*0.5*yStep[1] + y*yStep[1]; - yOffset[2] = -rows*0.5*yStep[2] + y*yStep[2]; - - sle.ray_pos_offset.x() = xOffset[0] + yOffset[0]; - sle.ray_pos_offset.y() = xOffset[1] + yOffset[1]; - sle.ray_pos_offset.z() = xOffset[2] + yOffset[2]; - - dGeomRaySet(sle.geom, pos[0] + xOffset[0] + yOffset[0], - pos[1] + xOffset[1] + yOffset[1], - pos[2] + xOffset[2] + yOffset[2], - dest[0], dest[1], dest[2]); - sle.gd = gd; - sle.index = y*cols+x; - sensor_list.push_back(sle); - dGeomSetData(sle.geom, gd); - //dGeomRaySetParams(sle.geom, 1, 1); - dGeomSetCollideBits(sle.geom, COLLIDE_MASK_SENSOR); - dGeomSetCategoryBits(sle.geom, COLLIDE_MASK_SENSOR); - dGeomDisable(sle.geom); - - } - } - } - } - - void NodePhysics::removeSensor(BaseSensor *sensor) { - MutexLocker locker(&(theWorld->iMutex)); - std::vector::iterator iter; - for (iter = sensor_list.begin(); iter != sensor_list.end(); ) { - if (iter->sensor == sensor) { - free(iter->gd); - dGeomDestroy(iter->geom); - iter = sensor_list.erase(iter); - } else - ++iter; - } - } - - /** - * \brief This function copies all sensor values to the specific allocated - * memory - * - * pre: - * - * post: - */ - void NodePhysics::handleSensorData(bool physics_thread) { - if(!physics_thread) return; - MutexLocker locker(&(theWorld->iMutex)); - std::vector::iterator iter; - const dReal* pos = dGeomGetPosition(nGeom); - const dReal* rot = dGeomGetRotation(nGeom); - dVector3 dest, tmp, posOffset; - dReal steps_size = 1.0, length = 0.0; - bool done = false; - int steps = 0; - dReal worldStep = theWorld->getWorldStep(); - // RotatingRaySensor - utils::Vector tmpV; - utils::Quaternion turnrotation; - turnrotation.setIdentity(); - std::set ids_rotating_ray_sensors; - - //New Code - int i=0; - for(iter = sensor_list.begin(); iter != sensor_list.end(); iter++) { - i+=1; - if((double)iter->sensor->updateRate * 0.001 > worldStep) { - iter->updateTime += worldStep; - if(iter->updateTime < 0.001*iter->sensor->updateRate) continue; - iter->updateTime -= 0.001*iter->sensor->updateRate; - } - BasePolarIntersectionSensor *polarSensor = dynamic_cast((*iter).sensor); - if(polarSensor){ - sensor_list_element elem = *iter; - - tmpV[0] = elem.ray_direction.x(); - tmpV[1] = elem.ray_direction.y(); - tmpV[2] = elem.ray_direction.z(); - - // Applies orientation_offset (z-Rotation) to the laser rays. - mars::sim::RotatingRaySensor *rotRaySensor = dynamic_cast((*iter).sensor); - if(rotRaySensor){ - std::set::iterator it = ids_rotating_ray_sensors.find(rotRaySensor->id); - // Takes care that each rotating ray sensor is only turned once (sensor_list contains each ray independently). - if(it == ids_rotating_ray_sensors.end()) { - //turnrotation.setIdentity(); // If we set it to identity first, receiveSensorData will not be called anymore.. wtf!? - turnrotation = rotRaySensor->turn(); - ids_rotating_ray_sensors.insert(rotRaySensor->id); - } - //fprintf(stderr, "tmp[%i]: %f, %f, %f\n", i, tmpV.x(), tmpV.y(), tmpV.z()); - tmpV = turnrotation * tmpV; - //fprintf(stderr, "tmp[%i]: %f, %f, %f\n", i, tmpV.x(), tmpV.y(), tmpV.z()); - } - tmp[0] = tmpV.x();//elem.ray_direction.x(); - tmp[1] = tmpV.y();//elem.ray_direction.y(); - tmp[2] = tmpV.z();//elem.ray_direction.z(); - dMULTIPLY0_331(dest, rot, tmp); - - if(physics_thread) { - steps = 0; - length = 0.0; - done = false; - dGeomEnable(elem.geom); - // make here the collision check - while (!done) { - dGeomRaySet(elem.geom, - pos[0] + dest[0]*steps_size*steps, - pos[1] + dest[1]*steps_size*steps, - pos[2] + dest[2]*steps_size*steps, - dest[0], dest[1], dest[2]); - if(length + steps_size < polarSensor->maxDistance) { - steps++; - dGeomRaySetLength(elem.geom, steps_size); - } - else { - dGeomRaySetLength(elem.geom, polarSensor->maxDistance- length); - done = true; - } - if(theWorld->handleCollision(elem.geom)) { - elem.gd->value += length; - done = true; - } - if(!done) length = steps_size*steps; - } - dGeomDisable(elem.geom); - } - (*polarSensor)[elem.index] = elem.gd->value; - elem.gd->value = polarSensor->maxDistance; - } - - BaseGridIntersectionSensor *polarGridSensor; - polarGridSensor = dynamic_cast(iter->sensor); - - if(polarGridSensor) { - sensor_list_element elem = *iter; - - if(physics_thread) { - - tmp[0] = elem.ray_direction.x(); - tmp[1] = elem.ray_direction.y(); - tmp[2] = elem.ray_direction.z(); - dMULTIPLY0_331(dest, rot, tmp); - - tmp[0] = elem.ray_pos_offset.x(); - tmp[1] = elem.ray_pos_offset.y(); - tmp[2] = elem.ray_pos_offset.z(); - dMULTIPLY0_331(posOffset, rot, tmp); - - dGeomEnable(elem.geom); - - dGeomRaySet(elem.geom, pos[0] + posOffset[0], - pos[1] + posOffset[1], - pos[2] + posOffset[2], - dest[0], dest[1], dest[2]); - - dGeomRaySetLength(elem.geom, polarGridSensor->maxDistance); - theWorld->handleCollision(elem.geom); - dGeomDisable(elem.geom); - } - (*polarGridSensor)[elem.index] = elem.gd->value; - elem.gd->value = polarGridSensor->maxDistance; - } - } // end for loop. - } - - /** - * \brief destroyes a node from the physics - * - * pre: - * - * post: - */ - void NodePhysics::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; - } - - void NodePhysics::setInertiaMass(NodeData* node) { - - dMassSetZero(&nMass); - nMass.mass = (dReal)node->mass; - nMass.I[0] = (dReal)node->inertia[0][0]; - nMass.I[1] = (dReal)node->inertia[0][1]; - nMass.I[2] = (dReal)node->inertia[0][2]; - nMass.I[3] = 0.0; - - nMass.I[4] = (dReal)node->inertia[1][0]; - nMass.I[5] = (dReal)node->inertia[1][1]; - nMass.I[6] = (dReal)node->inertia[1][2]; - nMass.I[7] = 0.0; - - nMass.I[8] = (dReal)node->inertia[2][0]; - nMass.I[9] = (dReal)node->inertia[2][1]; - nMass.I[10] = (dReal)node->inertia[2][2]; - nMass.I[11] = 0.0; - } - - void NodePhysics::getMass(sReal *mass, sReal *inertia) const { - if(mass) *mass = nMass.mass; - if(inertia) { - inertia[0] = nMass.I[0]; - inertia[1] = nMass.I[1]; - inertia[2] = nMass.I[2]; - inertia[3] = nMass.I[4]; - inertia[4] = nMass.I[5]; - inertia[5] = nMass.I[6]; - inertia[6] = nMass.I[8]; - inertia[7] = nMass.I[9]; - inertia[8] = nMass.I[10]; - } - } - - std::vector NodePhysics::addContacts(ContactsPhysics contacts, dWorldID world, dJointGroupID contactgroup){ - dVector3 v; - //dMatrix3 R; - dReal dot; - std::vector fbs; - std::shared_ptr> contactsPtr = contacts.contactsPtr; - const smurf::ContactParams contactParams = contacts.collidable->getContactParams(); - std::string nodeName = contacts.collidable->getName(); - for(int i=0; ioperator[](i).geom.normal[0]; - v[1] = contactsPtr->operator[](i).geom.normal[1]; - v[2] = contactsPtr->operator[](i).geom.normal[2]; - dot = dDOT(v, contactsPtr->operator[](i).fdir1); - dOPEC(v, *=, dot); - contactsPtr->operator[](i).fdir1[0] -= v[0]; - contactsPtr->operator[](i).fdir1[1] -= v[1]; - contactsPtr->operator[](i).fdir1[2] -= v[2]; - //dNormalize3(contactsPtr->operator[](0).fdir1); // Why 0 and not i? - dNormalize3(contactsPtr->operator[](i).fdir1); - //} - contactsPtr->operator[](i).geom.depth += (contactParams.depth_correction); - /* - // TODO: If after further testing this is not needed, then remove. - // 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; - contactsPtr->operator[](i).geom.depth = 0.001; - } - */ - if(contactsPtr->operator[](i).geom.depth < 0.0) contactsPtr->operator[](i).geom.depth = 0.0; - //contactsPtr->operator[](0).geom.depth += (contactParams.depth_correction); // Why 0 and not i? - contactsPtr->operator[](i).geom.depth += (contactParams.depth_correction); - //if(contactsPtr->operator[](0).geom.depth < 0.0) contactsPtr->operator[](0).geom.depth = 0.0; // Why 0 and not i ? - dJointID c=dJointCreateContact(world, contactgroup, &contactsPtr->operator[](i)); - dJointFeedback *fb; - fb = (dJointFeedback*)malloc(sizeof(dJointFeedback)); - dJointSetFeedback(c, fb); // ODE - #ifdef DEBUG_ADD_CONTACTS - if (isnan(abs(fb->f1[0]))||isnan(abs(fb->f1[1]))||isnan(abs(fb->f1[2])) || isnan(abs(fb->f1[3])) || - isnan(abs(fb->t1[0]))||isnan(abs(fb->t1[1]))||isnan(abs(fb->t1[2])) || isnan(abs(fb->t1[3])) || - isnan(abs(fb->f2[0]))||isnan(abs(fb->f2[1]))||isnan(abs(fb->f2[2])) || isnan(abs(fb->f2[3])) || - isnan(abs(fb->t2[0]))||isnan(abs(fb->t2[1]))||isnan(abs(fb->t2[2])) || isnan(abs(fb->t2[3])) ) - { - 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()); - #endif - fbs.push_back(fb); - addContact(c, contactsPtr->operator[](i), fb); - } // for numContacts - return fbs; - } - - void NodePhysics::addContact(dJointID contactJointId, dContact contact, dJointFeedback* fb){ - Vector contact_point; - dJointAttach(contactJointId, nBody, 0); - node_data.num_ground_collisions ++; - contact_point.x() = contact.geom.pos[0]; - contact_point.y() = contact.geom.pos[1]; - contact_point.z() = contact.geom.pos[2]; - node_data.contact_ids.push_back(0); - node_data.contact_points.push_back(contact_point); - node_data.ground_feedbacks.push_back(fb); - node_data.node1 = true; - } - - - sReal NodePhysics::getCollisionDepth(void) const { - if(nGeom && theWorld) { - return theWorld->getCollisionDepth(nGeom); - } - return 0.0; - } - - } // end of namespace sim -} // end of namespace mars diff --git a/sim/src/physics/NodePhysics.h b/sim/src/physics/NodePhysics.h deleted file mode 100644 index e876bc162..000000000 --- a/sim/src/physics/NodePhysics.h +++ /dev/null @@ -1,171 +0,0 @@ -/* - * 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 NodePhysics.h - * \author Malte Roemmermann - * \brief "NodePhysics" implements the physical ode stuff for the nodes. - * - */ - -#ifndef NODE_PHYSICS_H -#define NODE_PHYSICS_H - -#ifdef _PRINT_HEADER_ - #warning "NodePhysics.h" -#endif - -#include "WorldPhysics.h" - -#include - -#ifndef ODE11 - #define dTriIndex int -#endif - -namespace mars { - namespace sim { - - /* - * we need a data structure to handle different collision parameter - * and we need to save the collision_data somewhere - * in this first version, it is a hack to get the ground collisions - */ - struct geom_data { - void setZero(){ - num_ground_collisions = 0; - ray_sensor = 0; - sense_contact_force = 1; - value = 0; - c_params.setZero(); - filter_depth = -1.; - filter_angle = -1.; - filter_radius = -1.0; - } - - geom_data(){ - setZero(); - } - unsigned long id; - int num_ground_collisions; - std::vector contact_points; - std::list contact_ids; - std::vector ground_feedbacks; - bool node1; - interfaces::contact_params c_params; - bool ray_sensor; - bool sense_contact_force; - interfaces::sReal value; - dGeomID parent_geom; - dBodyID parent_body; - dReal filter_depth, filter_angle, filter_radius; - utils::Vector filter_sphere; - }; - - struct sensor_list_element { - interfaces::BaseSensor *sensor; - geom_data *gd; - dGeomID geom; - utils::Vector ray_direction; - utils::Vector ray_pos_offset; - unsigned int index; - dReal updateTime; - }; - - /** - * The class that implements the NodeInterface interface. - * - */ - class NodePhysics : public interfaces::NodeInterface { - public: - NodePhysics(std::shared_ptr world); - virtual ~NodePhysics(void); - virtual bool createNode(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; - virtual const utils::Quaternion setRotation(const utils::Quaternion &q, bool move_group); - virtual void setWorldObject(std::shared_ptr world); - virtual void getLinearVelocity(utils::Vector *vel) const; - virtual void getAngularVelocity(utils::Vector *vel) const; - virtual void getForce(utils::Vector *f) const; - virtual void getTorque(utils::Vector *t) const; - 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); - virtual void setTorque(const utils::Vector &t); - virtual void addForce(const utils::Vector &f, const utils::Vector &p); - virtual void addForce(const utils::Vector &f); - virtual void addTorque(const utils::Vector &t); - virtual bool getGroundContact(void) const; - virtual void getContactPoints(std::vector *contact_points) const; - virtual void getContactIDs(std::list *ids) const; - virtual interfaces::sReal getGroundContactForce(void) const; - virtual void setContactParams(interfaces::contact_params &c_params); - virtual void addSensor(interfaces::BaseSensor *sensor); - virtual void removeSensor(interfaces::BaseSensor *sensor); - virtual void handleSensorData(bool physics_thread = true); - virtual void destroyNode(void); - virtual void getMass(interfaces::sReal *mass, interfaces::sReal *inertia=0) const; - virtual const utils::Vector getContactForce(void) const; - virtual void addContact(dJointID contactJointId, dContact contact, dJointFeedback* fb); - virtual std::vector addContacts(ContactsPhysics contacts, dWorldID world, dJointGroupID contactgroup); - virtual interfaces::sReal getCollisionDepth(void) const; - void addCompositeOffset(dReal x, dReal y, dReal z); - ///return the body; this function is created to make it possible to get the - ///body from joint physics s - dBodyID getBody() const; - dMass getODEMass(void) const; - void addMassToCompositeBody(dBodyID theBody, dMass *bodyMass); - void getAbsMass(dMass *pMass) const; - dReal heightCallback(int x, int y); - - 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); - }; - - } // end of namespace sim -} // end of namespace mars - -#endif // NODE_PHYSICS_H diff --git a/sim/src/physics/WorldPhysics.cpp b/sim/src/physics/WorldPhysics.cpp index c05a22c7c..4bad971e7 100644 --- a/sim/src/physics/WorldPhysics.cpp +++ b/sim/src/physics/WorldPhysics.cpp @@ -37,7 +37,7 @@ */ #include "WorldPhysics.h" -#include "ODEObject.h" +#include "objects/ODEObject.h" #include "SimNode.h" diff --git a/sim/src/physics/JointFactoryInterface.h b/sim/src/physics/joints/JointFactoryInterface.h similarity index 100% rename from sim/src/physics/JointFactoryInterface.h rename to sim/src/physics/joints/JointFactoryInterface.h diff --git a/sim/src/physics/joints/ODEBallJoint.cpp b/sim/src/physics/joints/ODEBallJoint.cpp new file mode 100644 index 000000000..c4e4e0906 --- /dev/null +++ b/sim/src/physics/joints/ODEBallJoint.cpp @@ -0,0 +1,83 @@ +/* + * 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) { + } + + 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..030bb9741 --- /dev/null +++ b/sim/src/physics/joints/ODEBallJoint.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 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); + 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..743153209 --- /dev/null +++ b/sim/src/physics/joints/ODEFixedJoint.cpp @@ -0,0 +1,58 @@ +/* + * 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) { + } + + 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..1c0e56752 --- /dev/null +++ b/sim/src/physics/joints/ODEFixedJoint.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 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); + 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..738dc6498 --- /dev/null +++ b/sim/src/physics/joints/ODEHinge2Joint.cpp @@ -0,0 +1,81 @@ +/* + * 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) { + } + + 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..cfae3c9ed --- /dev/null +++ b/sim/src/physics/joints/ODEHinge2Joint.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 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); + 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..99265738f --- /dev/null +++ b/sim/src/physics/joints/ODEHingeJoint.cpp @@ -0,0 +1,77 @@ +/* + * 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) { + } + + 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..c49fede72 --- /dev/null +++ b/sim/src/physics/joints/ODEHingeJoint.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 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); + 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/ODEJoint.cpp b/sim/src/physics/joints/ODEJoint.cpp similarity index 99% rename from sim/src/physics/ODEJoint.cpp rename to sim/src/physics/joints/ODEJoint.cpp index 04b372e27..0953d646c 100644 --- a/sim/src/physics/ODEJoint.cpp +++ b/sim/src/physics/joints/ODEJoint.cpp @@ -31,7 +31,7 @@ #include #include "ODEJoint.h" -#include "ODEObject.h" +#include "objects/ODEObject.h" #include diff --git a/sim/src/physics/ODEJoint.h b/sim/src/physics/joints/ODEJoint.h similarity index 100% rename from sim/src/physics/ODEJoint.h rename to sim/src/physics/joints/ODEJoint.h diff --git a/sim/src/physics/ODEJointFactory.cpp b/sim/src/physics/joints/ODEJointFactory.cpp similarity index 85% rename from sim/src/physics/ODEJointFactory.cpp rename to sim/src/physics/joints/ODEJointFactory.cpp index b18431cdd..6654744da 100644 --- a/sim/src/physics/ODEJointFactory.cpp +++ b/sim/src/physics/joints/ODEJointFactory.cpp @@ -18,8 +18,13 @@ * */ -#include -#include +#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 @@ -31,7 +36,7 @@ using namespace ::mars::interfaces; ODEJointFactory& ODEJointFactory::Instance(){ static ODEJointFactory instance; return instance; -} +} ODEJointFactory::ODEJointFactory(){ } @@ -39,12 +44,12 @@ ODEJointFactory::ODEJointFactory(){ ODEJointFactory::~ODEJointFactory(){ } -std::shared_ptr ODEJointFactory::createJoint(std::shared_ptr worldPhysics, +std::shared_ptr ODEJointFactory::createJoint(std::shared_ptr worldPhysics, interfaces::JointData *jointData, - const std::shared_ptr node1, + const std::shared_ptr node1, const std::shared_ptr node2){ - std::shared_ptr newJoint; + std::shared_ptr newJoint; switch(jointData->type) { case JOINT_TYPE_HINGE: @@ -52,19 +57,19 @@ std::shared_ptr ODEJointFactory::createJoint(std::shared_ptr(worldPhysics, jointData, node1, node2); - break; + break; case JOINT_TYPE_SLIDER: newJoint = std::make_shared(worldPhysics, jointData, node1, node2); - break; + break; case JOINT_TYPE_BALL: newJoint = std::make_shared(worldPhysics, jointData, node1, node2); - break; + break; case JOINT_TYPE_UNIVERSAL: newJoint = std::make_shared(worldPhysics, jointData, node1, node2); - break; + break; case JOINT_TYPE_FIXED: newJoint = std::make_shared(worldPhysics, jointData, node1, node2); - break; + break; default: // no correct type is spezified, so no physically node will be created std::cout << "Unknown type of ODEJoint requested. No physical joint was created." << std::endl; diff --git a/sim/src/physics/ODEJointFactory.h b/sim/src/physics/joints/ODEJointFactory.h similarity index 97% rename from sim/src/physics/ODEJointFactory.h rename to sim/src/physics/joints/ODEJointFactory.h index 380a05349..260dbedd3 100644 --- a/sim/src/physics/ODEJointFactory.h +++ b/sim/src/physics/joints/ODEJointFactory.h @@ -27,7 +27,7 @@ #pragma once -#include +#include "joints/JointFactoryInterface.h" namespace mars{ namespace sim{ diff --git a/sim/src/physics/joints/ODESliderJoint.cpp b/sim/src/physics/joints/ODESliderJoint.cpp new file mode 100644 index 000000000..6b3ac7ce5 --- /dev/null +++ b/sim/src/physics/joints/ODESliderJoint.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 "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) { + } + + 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..3500aa1ec --- /dev/null +++ b/sim/src/physics/joints/ODESliderJoint.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 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); + 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..b0316d91b --- /dev/null +++ b/sim/src/physics/joints/ODEUniversalJoint.cpp @@ -0,0 +1,87 @@ +/* + * 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) { + } + + 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..98560f6c2 --- /dev/null +++ b/sim/src/physics/joints/ODEUniversalJoint.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 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); + 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/objects/ODEBox.cpp b/sim/src/physics/objects/ODEBox.cpp new file mode 100644 index 000000000..37c31b676 --- /dev/null +++ b/sim/src/physics/objects/ODEBox.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 "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) { + } + + 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..1940b685a --- /dev/null +++ b/sim/src/physics/objects/ODEBox.h @@ -0,0 +1,58 @@ +/* + * 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); + //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..be17b5639 --- /dev/null +++ b/sim/src/physics/objects/ODECapsule.cpp @@ -0,0 +1,67 @@ +/* + * 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) { + } + + 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..96f5aca71 --- /dev/null +++ b/sim/src/physics/objects/ODECapsule.h @@ -0,0 +1,56 @@ +/* + * 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); + 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..11494d30a --- /dev/null +++ b/sim/src/physics/objects/ODECylinder.cpp @@ -0,0 +1,67 @@ +/* + * 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) { + } + + 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..ddfffe8be --- /dev/null +++ b/sim/src/physics/objects/ODECylinder.h @@ -0,0 +1,56 @@ +/* + * 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); + 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..bad64d721 --- /dev/null +++ b/sim/src/physics/objects/ODEHeightField.cpp @@ -0,0 +1,89 @@ +/* + * 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); + } + + 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..a874cf560 --- /dev/null +++ b/sim/src/physics/objects/ODEHeightField.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 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); + 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..e7ad91f50 --- /dev/null +++ b/sim/src/physics/objects/ODEMesh.cpp @@ -0,0 +1,131 @@ +/* + * 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); + } + + 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..57282f485 --- /dev/null +++ b/sim/src/physics/objects/ODEMesh.h @@ -0,0 +1,61 @@ +/* + * 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); + 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/ODEObject.cpp b/sim/src/physics/objects/ODEObject.cpp similarity index 100% rename from sim/src/physics/ODEObject.cpp rename to sim/src/physics/objects/ODEObject.cpp diff --git a/sim/src/physics/ODEObject.h b/sim/src/physics/objects/ODEObject.h similarity index 100% rename from sim/src/physics/ODEObject.h rename to sim/src/physics/objects/ODEObject.h diff --git a/sim/src/physics/ODEObjectFactory.cpp b/sim/src/physics/objects/ODEObjectFactory.cpp similarity index 87% rename from sim/src/physics/ODEObjectFactory.cpp rename to sim/src/physics/objects/ODEObjectFactory.cpp index 133d59175..fcadb4622 100644 --- a/sim/src/physics/ODEObjectFactory.cpp +++ b/sim/src/physics/objects/ODEObjectFactory.cpp @@ -18,8 +18,14 @@ * */ -#include -#include +#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 @@ -31,7 +37,7 @@ using namespace ::mars::interfaces; ODEObjectFactory& ODEObjectFactory::Instance(){ static ODEObjectFactory instance; return instance; -} +} ODEObjectFactory::ODEObjectFactory(){ } @@ -49,22 +55,22 @@ std::shared_ptr ODEObjectFactory::createObject(std::shared_ptr(worldPhysics, nodeData); - break; + break; case NODE_TYPE_CYLINDER: newObject = std::make_shared(worldPhysics, nodeData); - break; + break; case NODE_TYPE_MESH: newObject = std::make_shared(worldPhysics, nodeData); - break; + break; case NODE_TYPE_PLANE: newObject = std::make_shared(worldPhysics, nodeData); - break; + break; case NODE_TYPE_SPHERE: newObject = std::make_shared(worldPhysics, nodeData); - break; + break; case NODE_TYPE_TERRAIN: newObject = std::make_shared(worldPhysics, nodeData); - break; + break; default: // no correct type is spezified, so no physically node will be created std::cout << "Unknown type of ODEObject requested. No physical node was created." << std::endl; diff --git a/sim/src/physics/ODEObjectFactory.h b/sim/src/physics/objects/ODEObjectFactory.h similarity index 96% rename from sim/src/physics/ODEObjectFactory.h rename to sim/src/physics/objects/ODEObjectFactory.h index 9df2406fc..70fd94823 100644 --- a/sim/src/physics/ODEObjectFactory.h +++ b/sim/src/physics/objects/ODEObjectFactory.h @@ -27,7 +27,7 @@ #pragma once -#include +#include "objects/ObjectFactoryInterface.h" namespace mars{ namespace sim{ diff --git a/sim/src/physics/objects/ODEPlane.cpp b/sim/src/physics/objects/ODEPlane.cpp new file mode 100644 index 000000000..581272fa1 --- /dev/null +++ b/sim/src/physics/objects/ODEPlane.cpp @@ -0,0 +1,44 @@ +/* + * 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) { + } + + 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..5e4b2c800 --- /dev/null +++ b/sim/src/physics/objects/ODEPlane.h @@ -0,0 +1,56 @@ +/* + * 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); + 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..d7f533732 --- /dev/null +++ b/sim/src/physics/objects/ODESphere.cpp @@ -0,0 +1,67 @@ +/* + * 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) { + } + + /** + * 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..9968e5ae3 --- /dev/null +++ b/sim/src/physics/objects/ODESphere.h @@ -0,0 +1,56 @@ +/* + * 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); + 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/ObjectFactoryInterface.h b/sim/src/physics/objects/ObjectFactoryInterface.h similarity index 100% rename from sim/src/physics/ObjectFactoryInterface.h rename to sim/src/physics/objects/ObjectFactoryInterface.h From 243a9b4eedc2bfc7eee8e599470d3efe9ed93b65 Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Fri, 30 Dec 2022 19:05:56 +0100 Subject: [PATCH 18/21] Handle primtive node types via object factory and string node type --- sim/src/core/NodeManager.cpp | 14 ++++++++ sim/src/physics/objects/ODEBox.cpp | 4 +++ sim/src/physics/objects/ODEBox.h | 3 +- sim/src/physics/objects/ODECapsule.cpp | 4 +++ sim/src/physics/objects/ODECapsule.h | 1 + sim/src/physics/objects/ODECylinder.cpp | 4 ++- sim/src/physics/objects/ODECylinder.h | 1 + sim/src/physics/objects/ODEHeightField.cpp | 4 +++ sim/src/physics/objects/ODEHeightField.h | 1 + sim/src/physics/objects/ODEMesh.cpp | 4 +++ sim/src/physics/objects/ODEMesh.h | 1 + sim/src/physics/objects/ODEObjectFactory.cpp | 38 +++++--------------- sim/src/physics/objects/ODEObjectFactory.h | 8 ++++- sim/src/physics/objects/ODEPlane.cpp | 4 +++ sim/src/physics/objects/ODEPlane.h | 1 + sim/src/physics/objects/ODESphere.cpp | 4 +++ sim/src/physics/objects/ODESphere.h | 1 + 17 files changed, 65 insertions(+), 32 deletions(-) diff --git a/sim/src/core/NodeManager.cpp b/sim/src/core/NodeManager.cpp index a6ae152c8..e80cdab22 100644 --- a/sim/src/core/NodeManager.cpp +++ b/sim/src/core/NodeManager.cpp @@ -30,6 +30,13 @@ #include "NodeManager.h" #include "JointManager.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,6 +82,13 @@ 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() diff --git a/sim/src/physics/objects/ODEBox.cpp b/sim/src/physics/objects/ODEBox.cpp index 37c31b676..506e26148 100644 --- a/sim/src/physics/objects/ODEBox.cpp +++ b/sim/src/physics/objects/ODEBox.cpp @@ -34,6 +34,10 @@ namespace sim { 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)) { diff --git a/sim/src/physics/objects/ODEBox.h b/sim/src/physics/objects/ODEBox.h index 1940b685a..ece9fa61e 100644 --- a/sim/src/physics/objects/ODEBox.h +++ b/sim/src/physics/objects/ODEBox.h @@ -47,9 +47,10 @@ namespace sim { 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; + virtual bool createODEGeometry(interfaces::NodeData *node) override; }; } // end of namespace sim diff --git a/sim/src/physics/objects/ODECapsule.cpp b/sim/src/physics/objects/ODECapsule.cpp index be17b5639..8838cdf21 100644 --- a/sim/src/physics/objects/ODECapsule.cpp +++ b/sim/src/physics/objects/ODECapsule.cpp @@ -34,6 +34,10 @@ namespace sim { 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" diff --git a/sim/src/physics/objects/ODECapsule.h b/sim/src/physics/objects/ODECapsule.h index 96f5aca71..815c2dc17 100644 --- a/sim/src/physics/objects/ODECapsule.h +++ b/sim/src/physics/objects/ODECapsule.h @@ -47,6 +47,7 @@ namespace sim { 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; }; diff --git a/sim/src/physics/objects/ODECylinder.cpp b/sim/src/physics/objects/ODECylinder.cpp index 11494d30a..88d5427f0 100644 --- a/sim/src/physics/objects/ODECylinder.cpp +++ b/sim/src/physics/objects/ODECylinder.cpp @@ -33,7 +33,9 @@ namespace sim { 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" diff --git a/sim/src/physics/objects/ODECylinder.h b/sim/src/physics/objects/ODECylinder.h index ddfffe8be..c3eb7184d 100644 --- a/sim/src/physics/objects/ODECylinder.h +++ b/sim/src/physics/objects/ODECylinder.h @@ -47,6 +47,7 @@ namespace sim { 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; }; diff --git a/sim/src/physics/objects/ODEHeightField.cpp b/sim/src/physics/objects/ODEHeightField.cpp index bad64d721..854fdb33d 100644 --- a/sim/src/physics/objects/ODEHeightField.cpp +++ b/sim/src/physics/objects/ODEHeightField.cpp @@ -37,6 +37,10 @@ namespace sim { 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); } diff --git a/sim/src/physics/objects/ODEHeightField.h b/sim/src/physics/objects/ODEHeightField.h index a874cf560..6c1c043d5 100644 --- a/sim/src/physics/objects/ODEHeightField.h +++ b/sim/src/physics/objects/ODEHeightField.h @@ -47,6 +47,7 @@ namespace sim { 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); diff --git a/sim/src/physics/objects/ODEMesh.cpp b/sim/src/physics/objects/ODEMesh.cpp index e7ad91f50..4da82f86a 100644 --- a/sim/src/physics/objects/ODEMesh.cpp +++ b/sim/src/physics/objects/ODEMesh.cpp @@ -37,6 +37,10 @@ namespace sim { 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); diff --git a/sim/src/physics/objects/ODEMesh.h b/sim/src/physics/objects/ODEMesh.h index 57282f485..3a62a9097 100644 --- a/sim/src/physics/objects/ODEMesh.h +++ b/sim/src/physics/objects/ODEMesh.h @@ -47,6 +47,7 @@ namespace sim { 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: diff --git a/sim/src/physics/objects/ODEObjectFactory.cpp b/sim/src/physics/objects/ODEObjectFactory.cpp index fcadb4622..42b1613a4 100644 --- a/sim/src/physics/objects/ODEObjectFactory.cpp +++ b/sim/src/physics/objects/ODEObjectFactory.cpp @@ -47,44 +47,24 @@ ODEObjectFactory::~ODEObjectFactory(){ std::shared_ptr ODEObjectFactory::createObject(std::shared_ptr worldPhysics, NodeData * nodeData){ - std::shared_ptr newObject; - - switch(nodeData->physicMode) { - case NODE_TYPE_BOX: - newObject = std::make_shared(worldPhysics, nodeData); - break; - case NODE_TYPE_CAPSULE: - newObject = std::make_shared(worldPhysics, nodeData); - break; - case NODE_TYPE_CYLINDER: - newObject = std::make_shared(worldPhysics, nodeData); - break; - case NODE_TYPE_MESH: - newObject = std::make_shared(worldPhysics, nodeData); - break; - case NODE_TYPE_PLANE: - newObject = std::make_shared(worldPhysics, nodeData); - break; - case NODE_TYPE_SPHERE: - newObject = std::make_shared(worldPhysics, nodeData); - break; - case NODE_TYPE_TERRAIN: - newObject = std::make_shared(worldPhysics, nodeData); - break; - default: - // no correct type is spezified, so no physically node will be created - std::cout << "Unknown type of ODEObject requested. No physical node was created." << std::endl; - return std::shared_ptr(nullptr); - break; + 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 index 70fd94823..64af008e1 100644 --- a/sim/src/physics/objects/ODEObjectFactory.h +++ b/sim/src/physics/objects/ODEObjectFactory.h @@ -28,20 +28,26 @@ #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); -protected: +private: ODEObjectFactory(); virtual ~ODEObjectFactory(); + std::map availableObjects; + }; } diff --git a/sim/src/physics/objects/ODEPlane.cpp b/sim/src/physics/objects/ODEPlane.cpp index 581272fa1..1591596c4 100644 --- a/sim/src/physics/objects/ODEPlane.cpp +++ b/sim/src/physics/objects/ODEPlane.cpp @@ -34,6 +34,10 @@ namespace sim { 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()); diff --git a/sim/src/physics/objects/ODEPlane.h b/sim/src/physics/objects/ODEPlane.h index 5e4b2c800..2bcf98c1b 100644 --- a/sim/src/physics/objects/ODEPlane.h +++ b/sim/src/physics/objects/ODEPlane.h @@ -47,6 +47,7 @@ namespace sim { 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; }; diff --git a/sim/src/physics/objects/ODESphere.cpp b/sim/src/physics/objects/ODESphere.cpp index d7f533732..013c612ea 100644 --- a/sim/src/physics/objects/ODESphere.cpp +++ b/sim/src/physics/objects/ODESphere.cpp @@ -34,6 +34,10 @@ namespace sim { 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. * diff --git a/sim/src/physics/objects/ODESphere.h b/sim/src/physics/objects/ODESphere.h index 9968e5ae3..c17d407a2 100644 --- a/sim/src/physics/objects/ODESphere.h +++ b/sim/src/physics/objects/ODESphere.h @@ -47,6 +47,7 @@ namespace sim { 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; }; From 39b5a31bc31b2468b75b6b92acec290dd4de2bff Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Fri, 30 Dec 2022 19:26:34 +0100 Subject: [PATCH 19/21] Handle primtive joint types via joint factory and string joint type --- sim/src/core/JointManager.cpp | 15 ++++++++ sim/src/physics/joints/ODEBallJoint.cpp | 7 ++++ sim/src/physics/joints/ODEBallJoint.h | 4 +++ sim/src/physics/joints/ODEFixedJoint.cpp | 7 ++++ sim/src/physics/joints/ODEFixedJoint.h | 4 +++ sim/src/physics/joints/ODEHinge2Joint.cpp | 7 ++++ sim/src/physics/joints/ODEHinge2Joint.h | 4 +++ sim/src/physics/joints/ODEHingeJoint.cpp | 7 ++++ sim/src/physics/joints/ODEHingeJoint.h | 4 +++ sim/src/physics/joints/ODEJointFactory.cpp | 38 ++++++-------------- sim/src/physics/joints/ODEJointFactory.h | 11 +++++- sim/src/physics/joints/ODESliderJoint.cpp | 7 ++++ sim/src/physics/joints/ODESliderJoint.h | 4 +++ sim/src/physics/joints/ODEUniversalJoint.cpp | 7 ++++ sim/src/physics/joints/ODEUniversalJoint.h | 4 +++ 15 files changed, 102 insertions(+), 28 deletions(-) diff --git a/sim/src/core/JointManager.cpp b/sim/src/core/JointManager.cpp index bb30b94db..d41209001 100644 --- a/sim/src/core/JointManager.cpp +++ b/sim/src/core/JointManager.cpp @@ -31,17 +31,25 @@ #include "JointManager.h" #include "NodeManager.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 { @@ -60,6 +68,13 @@ namespace sim { 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) { diff --git a/sim/src/physics/joints/ODEBallJoint.cpp b/sim/src/physics/joints/ODEBallJoint.cpp index c4e4e0906..f14c1a77d 100644 --- a/sim/src/physics/joints/ODEBallJoint.cpp +++ b/sim/src/physics/joints/ODEBallJoint.cpp @@ -37,6 +37,13 @@ namespace sim { 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); diff --git a/sim/src/physics/joints/ODEBallJoint.h b/sim/src/physics/joints/ODEBallJoint.h index 030bb9741..6b46b3ae3 100644 --- a/sim/src/physics/joints/ODEBallJoint.h +++ b/sim/src/physics/joints/ODEBallJoint.h @@ -50,6 +50,10 @@ namespace sim { 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; }; diff --git a/sim/src/physics/joints/ODEFixedJoint.cpp b/sim/src/physics/joints/ODEFixedJoint.cpp index 743153209..d8e976bf1 100644 --- a/sim/src/physics/joints/ODEFixedJoint.cpp +++ b/sim/src/physics/joints/ODEFixedJoint.cpp @@ -37,6 +37,13 @@ namespace sim { 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); diff --git a/sim/src/physics/joints/ODEFixedJoint.h b/sim/src/physics/joints/ODEFixedJoint.h index 1c0e56752..2c3f9ce30 100644 --- a/sim/src/physics/joints/ODEFixedJoint.h +++ b/sim/src/physics/joints/ODEFixedJoint.h @@ -50,6 +50,10 @@ namespace sim { 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; }; diff --git a/sim/src/physics/joints/ODEHinge2Joint.cpp b/sim/src/physics/joints/ODEHinge2Joint.cpp index 738dc6498..7a7006a41 100644 --- a/sim/src/physics/joints/ODEHinge2Joint.cpp +++ b/sim/src/physics/joints/ODEHinge2Joint.cpp @@ -37,6 +37,13 @@ namespace sim { 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){ diff --git a/sim/src/physics/joints/ODEHinge2Joint.h b/sim/src/physics/joints/ODEHinge2Joint.h index cfae3c9ed..4bfe0dfab 100644 --- a/sim/src/physics/joints/ODEHinge2Joint.h +++ b/sim/src/physics/joints/ODEHinge2Joint.h @@ -50,6 +50,10 @@ namespace sim { 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; }; diff --git a/sim/src/physics/joints/ODEHingeJoint.cpp b/sim/src/physics/joints/ODEHingeJoint.cpp index 99265738f..68db6b5c2 100644 --- a/sim/src/physics/joints/ODEHingeJoint.cpp +++ b/sim/src/physics/joints/ODEHingeJoint.cpp @@ -37,6 +37,13 @@ namespace sim { 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); diff --git a/sim/src/physics/joints/ODEHingeJoint.h b/sim/src/physics/joints/ODEHingeJoint.h index c49fede72..da2a03eac 100644 --- a/sim/src/physics/joints/ODEHingeJoint.h +++ b/sim/src/physics/joints/ODEHingeJoint.h @@ -50,6 +50,10 @@ namespace sim { 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; }; diff --git a/sim/src/physics/joints/ODEJointFactory.cpp b/sim/src/physics/joints/ODEJointFactory.cpp index 6654744da..0bf007d29 100644 --- a/sim/src/physics/joints/ODEJointFactory.cpp +++ b/sim/src/physics/joints/ODEJointFactory.cpp @@ -48,42 +48,26 @@ std::shared_ptr ODEJointFactory::createJoint(std::shared_ptr node1, const std::shared_ptr node2){ - - std::shared_ptr newJoint; - - switch(jointData->type) { - case JOINT_TYPE_HINGE: - newJoint = std::make_shared(worldPhysics, jointData, node1, node2); - break; - case JOINT_TYPE_HINGE2: - newJoint = std::make_shared(worldPhysics, jointData, node1, node2); - break; - case JOINT_TYPE_SLIDER: - newJoint = std::make_shared(worldPhysics, jointData, node1, node2); - break; - case JOINT_TYPE_BALL: - newJoint = std::make_shared(worldPhysics, jointData, node1, node2); - break; - case JOINT_TYPE_UNIVERSAL: - newJoint = std::make_shared(worldPhysics, jointData, node1, node2); - break; - case JOINT_TYPE_FIXED: - newJoint = std::make_shared(worldPhysics, jointData, node1, node2); - break; - default: - // no correct type is spezified, so no physically node will be created - std::cout << "Unknown type of ODEJoint requested. No physical joint was created." << std::endl; - return std::shared_ptr(nullptr); - break; + std::map::iterator it = availableJoints.find(jointData->jointType); + 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 index 260dbedd3..f62ced994 100644 --- a/sim/src/physics/joints/ODEJointFactory.h +++ b/sim/src/physics/joints/ODEJointFactory.h @@ -28,12 +28,18 @@ #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(); @@ -41,10 +47,13 @@ class ODEJointFactory : public JointFactoryInterface{ 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(); + virtual ~ODEJointFactory(); + std::map availableJoints; + }; } diff --git a/sim/src/physics/joints/ODESliderJoint.cpp b/sim/src/physics/joints/ODESliderJoint.cpp index 6b3ac7ce5..785ae1907 100644 --- a/sim/src/physics/joints/ODESliderJoint.cpp +++ b/sim/src/physics/joints/ODESliderJoint.cpp @@ -37,6 +37,13 @@ namespace sim { 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); diff --git a/sim/src/physics/joints/ODESliderJoint.h b/sim/src/physics/joints/ODESliderJoint.h index 3500aa1ec..27ce1950c 100644 --- a/sim/src/physics/joints/ODESliderJoint.h +++ b/sim/src/physics/joints/ODESliderJoint.h @@ -50,6 +50,10 @@ namespace sim { 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; }; diff --git a/sim/src/physics/joints/ODEUniversalJoint.cpp b/sim/src/physics/joints/ODEUniversalJoint.cpp index b0316d91b..559495887 100644 --- a/sim/src/physics/joints/ODEUniversalJoint.cpp +++ b/sim/src/physics/joints/ODEUniversalJoint.cpp @@ -37,6 +37,13 @@ namespace sim { 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); diff --git a/sim/src/physics/joints/ODEUniversalJoint.h b/sim/src/physics/joints/ODEUniversalJoint.h index 98560f6c2..44a3f8b9b 100644 --- a/sim/src/physics/joints/ODEUniversalJoint.h +++ b/sim/src/physics/joints/ODEUniversalJoint.h @@ -50,6 +50,10 @@ namespace sim { 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; }; From 9f22b35d5647feeecbb1dd307f2f596a2974230e Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Fri, 30 Dec 2022 20:50:00 +0100 Subject: [PATCH 20/21] Replace enum NodeType with string nodeType --- graphics/src/wrapper/OSGNodeStruct.cpp | 34 ++++++++----------- interfaces/src/NodeData.cpp | 20 +++++------ interfaces/src/NodeData.h | 31 +++++++---------- interfaces/src/sim/NodeManagerInterface.h | 2 +- .../src/ObstacleGenerator.cpp | 10 +++--- sim/src/core/NodeManager.cpp | 22 ++++++------ sim/src/core/NodeManager.h | 2 +- sim/src/core/SimNode.cpp | 10 +++--- sim/src/core/SimNode.h | 4 +-- sim/src/physics/objects/ODEObject.cpp | 4 +-- sim/src/sensors/ScanningSonar.cpp | 4 +-- 11 files changed, 65 insertions(+), 78 deletions(-) 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/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/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/src/core/NodeManager.cpp b/sim/src/core/NodeManager.cpp index e80cdab22..69fe12607 100644 --- a/sim/src/core/NodeManager.cpp +++ b/sim/src/core/NodeManager.cpp @@ -96,7 +96,7 @@ namespace mars { } NodeId NodeManager::createPrimitiveNode(const std::string &name, - NodeType type, + const std::string &type, bool moveable, const Vector &pos, const Vector &extension, @@ -132,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(); @@ -183,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; @@ -203,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"); @@ -308,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, @@ -386,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; @@ -2000,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 9363cc253..7acf31e4e 100644 --- a/sim/src/core/NodeManager.h +++ b/sim/src/core/NodeManager.h @@ -56,7 +56,7 @@ namespace mars { lib_manager::LibManager *theManager); 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(), 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/physics/objects/ODEObject.cpp b/sim/src/physics/objects/ODEObject.cpp index 3575d10cd..583b3ed9b 100644 --- a/sim/src/physics/objects/ODEObject.cpp +++ b/sim/src/physics/objects/ODEObject.cpp @@ -231,7 +231,7 @@ namespace sim { // 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(); @@ -240,7 +240,7 @@ namespace sim { dGeomSetPosition(nGeom, (dReal)node->pos.x(), (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->physicMode == NODE_TYPE_TERRAIN) { + if(node->nodeType == "terrain") { dGeomGetQuaternion(nGeom, t1); dQMultiply0(t2, tmp, t1); dNormalize4(t2); 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; From 850940f9491c9a19cedf16d1a2708e96e9d484dd Mon Sep 17 00:00:00 2001 From: Muhammad Haider Khan Lodhi Date: Sat, 31 Dec 2022 19:23:08 +0100 Subject: [PATCH 21/21] Bug Fix: Convert enum joint type to string --- sim/src/physics/joints/ODEJointFactory.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/sim/src/physics/joints/ODEJointFactory.cpp b/sim/src/physics/joints/ODEJointFactory.cpp index 0bf007d29..1e4171502 100644 --- a/sim/src/physics/joints/ODEJointFactory.cpp +++ b/sim/src/physics/joints/ODEJointFactory.cpp @@ -25,6 +25,7 @@ #include "joints/ODEBallJoint.h" #include "joints/ODEUniversalJoint.h" #include "joints/ODEFixedJoint.h" +#include #include @@ -48,7 +49,7 @@ std::shared_ptr ODEJointFactory::createJoint(std::shared_ptr node1, const std::shared_ptr node2){ - std::map::iterator it = availableJoints.find(jointData->jointType); + 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 + "\"" ); }