std::shared_ptr<NodeData> EnvirePhysics::getInertialNode(const smurf::Inertial& inertial, const envire::core::FrameId& frame) { NodeData * result = new NodeData; std::shared_ptr<NodeData> resultPtr(result); urdf::Inertial inertialUrdf = inertial.getUrdfInertial(); result->init(inertial.getName()); result->initPrimitive(mars::interfaces::NODE_TYPE_SPHERE, mars::utils::Vector(0.1, 0.1, 0.1), inertialUrdf.mass); result->groupID = inertial.getGroupId(); result->movable = true; result->inertia[0][0] = inertialUrdf.ixx; result->inertia[0][1] = inertialUrdf.ixy; result->inertia[0][2] = inertialUrdf.ixz; result->inertia[1][0] = inertialUrdf.ixy; result->inertia[1][1] = inertialUrdf.iyy; result->inertia[1][2] = inertialUrdf.iyz; result->inertia[2][0] = inertialUrdf.ixz; result->inertia[2][1] = inertialUrdf.iyz; result->inertia[2][2] = inertialUrdf.izz; result->inertia_set = true; result->c_params.coll_bitmask = 0; #ifdef DEBUG LOG_DEBUG("[EnvirePhysics::getInertialNode] Inertial object's mass: %f", result->mass); #endif result->density = 0.0; setPos(frame, resultPtr); return resultPtr; }
std::shared_ptr<NodeData> EnvirePhysics::getCollidableNode(const smurf::Collidable& collidable, const envire::core::FrameId& frame) { NodeData * node = new NodeData; std::shared_ptr<NodeData> nodePtr(node); urdf::Collision collision = collidable.getCollision(); node->init(collision.name); node->fromGeometry(collision.geometry); node->density = 0.0; node->mass = 0.00001; setPos(frame, nodePtr); node->movable = true; node->c_params.fromSmurfCP(collidable.getContactParams()); node->groupID = collidable.getGroupId(); return nodePtr; }
void EnvireSmurfLoader::addFloor(const vertex_descriptor ¢er) { NodeData data; data.init("floorData", Vector(0,0,0)); data.initPrimitive(interfaces::NODE_TYPE_BOX, Vector(25, 25, 0.1), 0.0001); data.movable = false; mars::sim::PhysicsConfigMapItem::Ptr item(new mars::sim::PhysicsConfigMapItem); data.material.transparency = 0.5; //data.material.ambientFront = mars::utils::Color(0.0, 1.0, 0.0, 1.0); // TODO Fix the material data is lost in the conversion from/to configmap data.material.emissionFront = mars::utils::Color(1.0, 1.0, 1.0, 1.0); LOG_DEBUG("Color of the Item in the addFloor: %f , %f, %f, %f", data.material.emissionFront.a , data.material.emissionFront.b, data.material.emissionFront.g, data.material.emissionFront.r ); data.toConfigMap(&(item.get()->getData())); control->graph->addItemToFrame(control->graph->getFrameId(center), item); }
void EnvirePhysics::itemAdded(const TypedItemAddedEvent<Item<urdf::Collision>>& e) { #ifdef DEBUG LOG_DEBUG(("[EnvirePhysics::ItemAdded] urdf::Collision item received in frame *** " + e.frame + "***").c_str()); #endif urdf::Collision collision = e.item->getData(); NodeData * node = new NodeData; node->init(collision.name); node->fromGeometry(collision.geometry); node->mass = 0.00001; node->density = 0.0; std::shared_ptr<NodeData> nodePtr(node); setPos(e.frame, nodePtr); node->movable = true; if (instantiateNode(nodePtr, e.frame)) { #ifdef DEBUG LOG_DEBUG(("[EnvirePhysics::ItemAdded] Smurf::Collision - Instantiated and stored the nodeInterface in frame ***" + e.frame +"***").c_str()); #endif } }