Link* VRMLBodyLoaderImpl::readJointNode(VRMLProtoInstance* jointNode, const Matrix3& parentRs) { putVerboseMessage(string("Joint node") + jointNode->defName); Link* link = createLink(jointNode, parentRs); LinkInfo iLink; iLink.link = link; SgInvariantGroupPtr shapeTop = new SgInvariantGroup(); if(link->Rs().isApprox(Matrix3::Identity())){ iLink.shape = shapeTop.get(); } else { SgPosTransform* transformRs = new SgPosTransform; transformRs->setRotation(link->Rs()); shapeTop->addChild(transformRs); iLink.shape = transformRs; } iLink.m = 0.0; iLink.c = Vector3::Zero(); iLink.I = Matrix3::Zero(); MFNode& childNodes = get<MFNode>(jointNode->fields["children"]); Affine3 T(Affine3::Identity()); ProtoIdSet acceptableProtoIds; acceptableProtoIds.set(PROTO_JOINT); acceptableProtoIds.set(PROTO_SEGMENT); acceptableProtoIds.set(PROTO_DEVICE); readJointSubNodes(iLink, childNodes, acceptableProtoIds, T); Matrix3& I = iLink.I; for(size_t i=0; i < iLink.segments.size(); ++i){ const SegmentInfo& segment = iLink.segments[i]; const Vector3 o = segment.c - iLink.c; const double& x = o.x(); const double& y = o.y(); const double& z = o.z(); const double& m = segment.m; I(0,0) += m * (y * y + z * z); I(0,1) += -m * (x * y); I(0,2) += -m * (x * z); I(1,0) += -m * (y * x); I(1,1) += m * (z * z + x * x); I(1,2) += -m * (y * z); I(2,0) += -m * (z * x); I(2,1) += -m * (z * y); I(2,2) += m * (x * x + y * y); } link->setMass(iLink.m); link->setCenterOfMass(link->Rs() * iLink.c); link->setInertia(link->Rs() * iLink.I * link->Rs().transpose()); if(iLink.shape->empty()){ link->setShape(new SgNode()); // set empty node } else { link->setShape(shapeTop); } return link; }
Link* VRMLBodyLoaderImpl::readJointNode(VRMLProtoInstance* jointNode, const Matrix3& parentRs) { if(isVerbose) putMessage(string("Joint node") + jointNode->defName); Link* link = createLink(jointNode, parentRs); LinkInfo iLink; iLink.link = link; iLink.m = 0.0; iLink.c = Vector3::Zero(); iLink.I = Matrix3::Zero(); iLink.visualShape = new SgGroup; iLink.collisionShape = new SgGroup; iLink.isSurfaceNodeUsed = false; MFNode& childNodes = get<MFNode>(jointNode->fields["children"]); Affine3 T(Affine3::Identity()); ProtoIdSet acceptableProtoIds; acceptableProtoIds.set(PROTO_JOINT); acceptableProtoIds.set(PROTO_SEGMENT); acceptableProtoIds.set(PROTO_DEVICE); readJointSubNodes(iLink, childNodes, acceptableProtoIds, T); Matrix3& I = iLink.I; for(size_t i=0; i < iLink.segments.size(); ++i){ const SegmentInfo& segment = iLink.segments[i]; const Vector3 o = segment.c - iLink.c; const double& x = o.x(); const double& y = o.y(); const double& z = o.z(); const double& m = segment.m; I(0,0) += m * (y * y + z * z); I(0,1) += -m * (x * y); I(0,2) += -m * (x * z); I(1,0) += -m * (y * x); I(1,1) += m * (z * z + x * x); I(1,2) += -m * (y * z); I(2,0) += -m * (z * x); I(2,1) += -m * (z * y); I(2,2) += m * (x * x + y * y); } link->setMass(iLink.m); link->setCenterOfMass(link->Rs() * iLink.c); link->setInertia(link->Rs() * iLink.I * link->Rs().transpose()); setShape(link, iLink.visualShape, true); if(iLink.isSurfaceNodeUsed){ setShape(link, iLink.collisionShape, false); } else { link->setCollisionShape(link->visualShape()); } return link; }
Link* VRMLBodyLoaderImpl::createLink(VRMLProtoInstance* jointNode, const Matrix3& parentRs) { Link* link = body->createLink(); link->setName(jointNode->defName); VRMLProtoFieldMap& jf = jointNode->fields; link->setJointId(get<SFInt32>(jf["jointId"])); if(link->jointId() >= 0){ if(link->jointId() >= validJointIdSet.size()){ validJointIdSet.resize(link->jointId() + 1); } if(!validJointIdSet[link->jointId()]){ ++numValidJointIds; validJointIdSet.set(link->jointId()); } else { os() << str(format("Warning: Joint ID %1% is duplicated.") % link->jointId()) << endl; } } if(jointNode != rootJointNode){ Vector3 b; readVRMLfield(jf["translation"], b); link->setOffsetTranslation(parentRs * b); Matrix3 R; readVRMLfield(jf["rotation"], R); link->setAccumlatedSegmentRotation(parentRs * R); } string jointType; readVRMLfield(jf["jointType"], jointType); if(jointType == "fixed" ){ link->setJointType(Link::FIXED_JOINT); } else if(jointType == "free" ){ link->setJointType(Link::FREE_JOINT); } else if(jointType == "rotate" ){ link->setJointType(Link::ROTATIONAL_JOINT); } else if(jointType == "slide" ){ link->setJointType(Link::SLIDE_JOINT); } else if(jointType == "crawler"){ link->setJointType(Link::CRAWLER_JOINT); } else { link->setJointType(Link::FIXED_JOINT); } if(link->jointType() == Link::FREE_JOINT || link->jointType() == Link::FIXED_JOINT){ link->setJointAxis(Vector3::Zero()); } else { Vector3 jointAxis; VRMLVariantField& jointAxisField = jf["jointAxis"]; switch(jointAxisField.which()){ case SFSTRING: { SFString& axisLabel = get<SFString>(jointAxisField); if(axisLabel == "X"){ jointAxis = Vector3::UnitX(); } else if(axisLabel == "Y"){ jointAxis = Vector3::UnitY(); } else if(axisLabel == "Z"){ jointAxis = Vector3::UnitZ(); } } break; case SFVEC3F: readVRMLfield(jointAxisField, jointAxis); break; default: jointAxis = Vector3::UnitZ(); break; } link->setJointAxis(link->Rs() * jointAxis); } double Ir, gearRatio, torqueConst, encoderPulse, rotorResistor; readVRMLfield(jf["rotorInertia"], Ir); readVRMLfield(jf["gearRatio"], gearRatio); readVRMLfield(jf["torqueConst"], torqueConst); readVRMLfield(jf["encoderPulse"], encoderPulse); readVRMLfield(jf["rotorResistor"], rotorResistor); VRMLVariantField* field = jointNode->findField("equivalentInertia"); if(field){ link->setEquivalentRotorInertia(get<SFFloat>(*field)); } else { link->setEquivalentRotorInertia(gearRatio * gearRatio * Ir); } double maxlimit = numeric_limits<double>::max(); link->setJointRange( getLimitValue(jf["llimit"], -maxlimit), getLimitValue(jf["ulimit"], +maxlimit)); link->setJointVelocityRange( getLimitValue(jf["lvlimit"], -maxlimit), getLimitValue(jf["uvlimit"], +maxlimit)); return link; }