private void _setRootJoint(GrxModelItem robot) { Transform3D t3d = new Transform3D(); from_.tg_.getTransform(t3d); Transform3D t3dFromNew = new Transform3D(); from_.tg_.getLocalToVworld(t3dFromNew); t3dFromNew.mul(t3d); t3dFromNew.invert(); Transform3D t3dRoot = new Transform3D(); robot.getTransformGroupRoot().getTransform(t3dRoot); t3d.mul(trFrom_, t3dFromNew); t3d.mul(t3dRoot); Matrix3d rot = new Matrix3d(); Vector3d pos = new Vector3d(); t3d.get(rot, pos); robot.setTransformRoot(pos, rot); robot.updateInitialTransformRoot(); /* AxisAngle4d a4d = new AxisAngle4d(); a4d.set(rot); robot.setProperty(robot_.lInfo_[0].name+".rotation", +a4d.angle+" "+a4d.x+" "+a4d.y+" "+a4d.z); robot.setProperty(robot_.lInfo_[0].name+".translation", pos.x+" "+pos.y+" "+pos.z);*/ }