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; }
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); }