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