示例#1
0
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;
}
示例#2
0
 void EnvireSmurfLoader::addFloor(const vertex_descriptor &center)
 {
     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);
 }