Beispiel #1
0
        void ComputeParentIndices(URDFImporterInterface u2b, URDF2BulletCachedData cache, int urdfLinkIndex, int urdfParentIndex)
        {
            cache.m_urdfLinkParentIndices[urdfLinkIndex]             = urdfParentIndex;
            cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex] = cache.m_currentMultiBodyLinkIndex++;

            List <int> childIndices = new List <int>();

            u2b.getLinkChildIndices(urdfLinkIndex, childIndices);
            for (int i = 0; i < childIndices.Count; i++)
            {
                ComputeParentIndices(u2b, cache, childIndices[i], urdfLinkIndex);
            }
        }
Beispiel #2
0
        void ComputeTotalNumberOfJoints(URDFImporterInterface u2b, URDF2BulletCachedData cache, int linkIndex)
        {
            List <int> childIndices = new List <int>();

            u2b.getLinkChildIndices(linkIndex, childIndices);
            //b3Printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
            //for (int i=0;i<childIndices.size();i++)
            //{
            //    b3Printf("child %d has childIndex%d=%s\n",i,childIndices[i],u2b.getLinkName(childIndices[i]).c_str());
            //}
            cache.m_totalNumJoints1 += childIndices.Count;
            for (int i = 0; i < childIndices.Count; i++)
            {
                int childIndex = childIndices[i];
                ComputeTotalNumberOfJoints(u2b, cache, childIndex);
            }
        }
Beispiel #3
0
        public void InitURDF2BulletCache(URDFImporterInterface u2b, URDF2BulletCachedData cache)
        {
            //compute the number of links, and compute parent indices array (and possibly other cached data?)
            cache.m_totalNumJoints1 = 0;

            int rootLinkIndex = u2b.getRootLinkIndex();

            if (rootLinkIndex >= 0)
            {
                ComputeTotalNumberOfJoints(u2b, cache, rootLinkIndex);
                int numTotalLinksIncludingBase = 1 + cache.m_totalNumJoints1;

                Resize(cache.m_urdfLinkParentIndices, numTotalLinksIncludingBase);
                Resize(cache.m_urdfLinkIndices2BulletLinkIndices, numTotalLinksIncludingBase);
                Resize(cache.m_urdfLink2rigidBodies, numTotalLinksIncludingBase);
                Resize(cache.m_urdfLinkLocalInertialFrames, numTotalLinksIncludingBase);

                cache.m_currentMultiBodyLinkIndex = -1;//multi body base has 'link' index -1
                ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
            }
        }
Beispiel #4
0
        public void ConvertURDF2BulletInternal(
            URDFImporterInterface u2b,
            MultiBodyCreationInterface creation,
            URDF2BulletCachedData cache, int urdfLinkIndex,
            Matrix parentTransformInWorldSpace,
            GameObject parentGameObject,
            MultiBodyDynamicsWorld world1,
            bool createMultiBody, string pathPrefix,
            bool enableConstraints,
            ConvertURDFFlags flags = 0)
        {
            Matrix    linkTransformInWorldSpace = Matrix.Identity;
            int       mbLinkIndex     = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
            int       urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
            int       mbParentIndex   = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
            RigidBody parentRigidBody = null;

            //b3Printf();
            if (debugLevel >= BDebug.DebugType.Debug)
            {
                Debug.LogFormat("mb link index = {0}\n", mbLinkIndex);
            }

            Matrix parentLocalInertialFrame = Matrix.Identity;
            float  parentMass = (1);

            BulletSharp.Math.Vector3 parentLocalInertiaDiagonal = new BulletSharp.Math.Vector3(1, 1, 1);

            if (urdfParentIndex == -2)
            {
                if (debugLevel >= BDebug.DebugType.Debug)
                {
                    Debug.LogFormat("root link has no parent\n");
                }
            }
            else
            {
                if (debugLevel >= BDebug.DebugType.Debug)
                {
                    Debug.LogFormat("urdf parent index = {0}", urdfParentIndex);
                    Debug.LogFormat("mb parent index = {0}", mbParentIndex);
                }
                parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
                u2b.getMassAndInertia(urdfParentIndex, out parentMass, out parentLocalInertiaDiagonal, out parentLocalInertialFrame);
            }

            float  mass = 0;
            Matrix localInertialFrame = Matrix.Identity;

            BulletSharp.Math.Vector3 localInertiaDiagonal = new BulletSharp.Math.Vector3(0, 0, 0);
            u2b.getMassAndInertia(urdfLinkIndex, out mass, out localInertiaDiagonal, out localInertialFrame);
            Matrix parent2joint = Matrix.Identity;

            UrdfJointTypes jointType;

            BulletSharp.Math.Vector3 jointAxisInJointSpace;
            float jointLowerLimit;
            float jointUpperLimit;
            float jointDamping;
            float jointFriction;
            float jointMaxForce;
            float jointMaxVelocity;

            bool   hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, out parent2joint, out linkTransformInWorldSpace, out jointAxisInJointSpace, out jointType, out jointLowerLimit, out jointUpperLimit, out jointDamping, out jointFriction, out jointMaxForce, out jointMaxVelocity);
            string linkName       = u2b.getLinkName(urdfLinkIndex);

            if ((flags & ConvertURDFFlags.CUF_USE_SDF) != 0)
            {
                Matrix tmp = new Matrix();
                Matrix.Invert(ref parentTransformInWorldSpace, out tmp);
                Matrix.Multiply(ref tmp, ref linkTransformInWorldSpace, out parent2joint);
            }
            else
            {
                if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0)
                {
                    linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
                }
                else
                {
                    linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
                }
            }

            GameObject gameObject = new GameObject(linkName);

            if (parentGameObject != null)
            {
                gameObject.transform.parent = parentGameObject.transform;
            }
            //--------------------

            /*
             * bool hasParentJoint = loader.getJointInfo2(linkIndex, out parent2joint, out linkTransformInWorldSpace, out jointAxisInJointSpace, out jointType, out jointLowerLimit, out jointUpperLimit, out jointDamping, out jointFriction, out jointMaxForce, out jointMaxVelocity);
             * string linkName = loader.getLinkName(linkIndex);
             *
             * if ((flags & ConvertURDFFlags.CUF_USE_SDF) != 0)
             * {
             *  Matrix tmp = parentTransformInWorldSpace.Inverse();
             *  Matrix.Multiply(ref tmp, ref linkTransformInWorldSpace, out parent2joint);
             * }
             * else
             * {
             *  if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0)
             *  {
             *      linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
             *  }
             *  else
             *  {
             *      linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
             *  }
             * }
             *
             * lgo.transform.position = linkTransformInWorldSpace.Origin.ToUnity();
             */
            //--------------------
            gameObject.transform.position = linkTransformInWorldSpace.Origin.ToUnity();
            gameObject.transform.rotation = linkTransformInWorldSpace.Rotation.ToUnity();

            BCollisionShape compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex, pathPrefix, ref localInertialFrame, gameObject);


            int graphicsIndex;

            {
                graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex, pathPrefix, ref localInertialFrame);
            }

            if (compoundShape != null)
            {
                UrdfMaterialColor matColor;
                Color             color2   = Color.red;
                Color             specular = new Color(0.5f, 0.5f, 0.5f);
                if (u2b.getLinkColor2(urdfLinkIndex, out matColor))
                {
                    color2   = matColor.m_rgbaColor;
                    specular = matColor.m_specularColor;
                }

                if (mass != 0)
                {
                    if ((flags & ConvertURDFFlags.CUF_USE_URDF_INERTIA) == 0)
                    {
                        compoundShape.GetCollisionShape().CalculateLocalInertia(mass, out localInertiaDiagonal);
                        Debug.Assert(localInertiaDiagonal[0] < 1e10);
                        Debug.Assert(localInertiaDiagonal[1] < 1e10);
                        Debug.Assert(localInertiaDiagonal[2] < 1e10);
                    }
                    URDFLinkContactInfo contactInfo;
                    u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo);
                    //temporary inertia scaling until we load inertia from URDF
                    if ((contactInfo.m_flags & URDF_LinkContactFlags.URDF_CONTACT_HAS_INERTIA_SCALING) != 0)
                    {
                        localInertiaDiagonal *= contactInfo.m_inertiaScaling;
                    }
                }

                RigidBody linkRigidBody             = null;
                Matrix    inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame;

                if (!createMultiBody)
                {
                    RigidBody body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape.GetCollisionShape());
                    linkRigidBody = body;
                    world1.AddRigidBody(body);
                    compoundShape.GetCollisionShape().UserIndex = (graphicsIndex);
                    URDFLinkContactInfo contactInfo;
                    u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo);
                    ProcessContactParameters(contactInfo, body);
                    creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2, specular, graphicsIndex);
                    cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape.GetCollisionShape(), ref localInertialFrame);
                    //untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
                }
                else
                {
                    if (cache.m_bulletMultiBody == null)
                    {
                        // creating base
                        bool canSleep       = false;
                        bool isFixedBase    = (mass == 0);//todo: figure out when base is fixed
                        int  totalNumJoints = cache.m_totalNumJoints1;
                        //cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
                        BMultiBody bmm = cache.m_bulletMultiBody = gameObject.AddComponent <BMultiBody>();
                        bmm.fixedBase = isFixedBase;
                        bmm.canSleep  = canSleep;
                        bmm.baseMass  = mass;
                        //new btMultiBody(totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
                        //if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0)
                        //{
                        //    cache.m_bulletMultiBody.BaseWorldTransform = (linkTransformInWorldSpace);
                        //}
                        //cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape.GetCollisionShape(), ref localInertialFrame);
                    }
                }

                //create a joint if necessary
                BMultiBodyLink bmbl = null;
                if (hasParentJoint)
                {
                    //====================
                    //btTransform offsetInA, offsetInB;
                    //offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
                    //offsetInB = localInertialFrame.inverse();
                    //btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
                    //=====================
                    Matrix offsetInA = new Matrix(), offsetInB = new Matrix();
                    Matrix.Invert(ref parentLocalInertialFrame, out offsetInA);
                    offsetInA = offsetInA * parent2joint;
                    Matrix.Invert(ref localInertialFrame, out offsetInB);
                    Matrix offsetInAInv = new Matrix();
                    Matrix.Invert(ref offsetInA, out offsetInAInv);
                    BulletSharp.Math.Quaternion parentRotToThis = offsetInB.GetRotation() * offsetInAInv.GetRotation();

                    Matrix tmp = new Matrix();
                    Matrix.Invert(ref parentTransformInWorldSpace, out tmp);
                    Matrix tmp2 = new Matrix();
                    Matrix.Invert(ref parent2joint, out tmp2);
                    Matrix tmp3       = new Matrix();
                    Matrix tmp4       = linkTransformInWorldSpace;
                    Matrix link2joint = new Matrix();
                    Matrix.Multiply(ref tmp, ref tmp2, out tmp3);
                    Matrix.Multiply(ref tmp3, ref tmp4, out link2joint);

                    if (debugLevel >= BDebug.DebugType.Debug)
                    {
                        Matrix linkTransformInWorldSpaceInv = new Matrix(), parentTransformInWorldSpaceInv = new Matrix();
                        Matrix.Invert(ref linkTransformInWorldSpace, out linkTransformInWorldSpaceInv);
                        Matrix.Invert(ref parentTransformInWorldSpace, out parentTransformInWorldSpaceInv);
                        Debug.Log("Creating link " + linkName + " offsetInA=" + offsetInA.Origin + " offsetInB=" + offsetInB.Origin + " parentLocalInertialFrame=" + parentLocalInertialFrame.Origin + " localInertialFrame=" + localInertialFrame.Origin + " localTrnaform=" +
                                  " linkTransInWorldSpace=" + linkTransformInWorldSpace.Origin +
                                  " linkTransInWorldSpaceInv=" + linkTransformInWorldSpaceInv.Origin +
                                  " parentTransInWorldSpace=" + parentTransformInWorldSpace.Origin +
                                  " parentTransInWorldSpaceInv=" + parentTransformInWorldSpaceInv.Origin +
                                  " link2joint=" + link2joint.Origin +
                                  " jointType=" + jointType);
                    }

                    bool disableParentCollision = true;

                    switch (jointType)
                    {
                    case UrdfJointTypes.URDFFloatingJoint:
                    case UrdfJointTypes.URDFPlanarJoint:
                    case UrdfJointTypes.URDFFixedJoint:
                    {
                        if ((jointType == UrdfJointTypes.URDFFloatingJoint) || (jointType == UrdfJointTypes.URDFPlanarJoint))
                        {
                            Debug.Log("Warning: joint unsupported, creating a fixed joint instead.");
                        }
                        //creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);

                        if (createMultiBody)
                        {
                            //todo: adjust the center of mass transform and pivot axis properly

                            /*
                             * cache.m_bulletMultiBody.SetupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
                             *                                  parentRotToThis, offsetInA.Origin, -offsetInB.Origin);
                             */
                            bmbl                    = gameObject.AddComponent <BMultiBodyLink>();
                            bmbl.jointType          = FeatherstoneJointType.Fixed;
                            bmbl.mass               = mass;
                            bmbl.localPivotPosition = offsetInB.Origin.ToUnity();
                        }
                        else
                        {
                            //b3Printf("Fixed joint\n");
                            Debug.LogError("TODO Setup 6dof ");

                            /*
                             * Generic6DofSpring2Constraint dof6 = null;
                             *
                             * //backward compatibility
                             * if ((flags & ConvertURDFFlags.CUF_RESERVED) != 0)
                             * {
                             *  dof6 = creation.createFixedJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB);
                             * }
                             * else
                             * {
                             *  dof6 = creation.createFixedJoint(urdfLinkIndex, linkRigidBody, parentRigidBody, offsetInB, offsetInA);
                             * }
                             * if (enableConstraints)
                             *  world1.AddConstraint(dof6, true);
                             */
                        }
                        break;
                    }

                    case UrdfJointTypes.URDFContinuousJoint:
                    case UrdfJointTypes.URDFRevoluteJoint:
                    {
                        //creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
                        if (createMultiBody)
                        {
                            //cache.m_bulletMultiBody.SetupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
                            //                                          parentRotToThis, (offsetInB.GetRotation().Rotate(jointAxisInJointSpace)), offsetInA.Origin,//parent2joint.getOrigin(),
                            //                                          -offsetInB.Origin,
                            //                                          disableParentCollision);
                            bmbl                    = gameObject.AddComponent <BMultiBodyLink>();
                            bmbl.jointType          = FeatherstoneJointType.Revolute;
                            bmbl.mass               = mass;
                            bmbl.localPivotPosition = offsetInB.Origin.ToUnity();
                            bmbl.rotationAxis       = offsetInB.Rotation.Rotate(jointAxisInJointSpace).ToUnity();
                            if (jointType == UrdfJointTypes.URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
                            {
                                //string name = u2b.getLinkName(urdfLinkIndex);
                                //printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
                                BMultiBodyJointLimitConstraint mbc = gameObject.AddComponent <BMultiBodyJointLimitConstraint>();
                                mbc.m_jointLowerLimit = jointLowerLimit;
                                mbc.m_jointUpperLimit = jointUpperLimit;
                            }

                            /*
                             * Debug.Log("=========== Creating joint for: " + gameObject.name);
                             * Debug.Log("parentRotateToThis: " + parentRotToThis.ToUnity().eulerAngles);
                             * Debug.Log("rotationAxis: " + bmbl.rotationAxis);
                             * Debug.Log("offsetInA: " + offsetInA.Origin);
                             * Debug.Log("negOffsetInB: " + -offsetInB.Origin);
                             */
                        }
                        else
                        {
                            Generic6DofSpring2Constraint dof6 = null;
                            //backwards compatibility
                            if ((flags & ConvertURDFFlags.CUF_RESERVED) != 0)
                            {
                                dof6 = creation.createRevoluteJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
                            }
                            else
                            {
                                dof6 = creation.createRevoluteJoint(urdfLinkIndex, linkRigidBody, parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
                            }
                            if (enableConstraints)
                            {
                                world1.AddConstraint(dof6, true);
                            }
                            //b3Printf("Revolute/Continuous joint\n");
                        }

                        break;
                    }

                    case UrdfJointTypes.URDFPrismaticJoint:
                    {
                        //creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);

                        if (createMultiBody)
                        {
                            //cache.m_bulletMultiBody.SetupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
                            //                                           parentRotToThis, (offsetInB.GetRotation().Rotate(jointAxisInJointSpace)), offsetInA.Origin,//parent2joint.getOrigin(),
                            //                                           -offsetInB.Origin,
                            //                                           disableParentCollision);
                            bmbl                    = gameObject.AddComponent <BMultiBodyLink>();
                            bmbl.jointType          = FeatherstoneJointType.Prismatic;
                            bmbl.mass               = mass;
                            bmbl.localPivotPosition = offsetInB.Origin.ToUnity();
                            bmbl.rotationAxis       = offsetInB.Rotation.Rotate(jointAxisInJointSpace).ToUnity();

                            if (jointLowerLimit <= jointUpperLimit)
                            {
                                //string name = u2b.getLinkName(urdfLinkIndex);
                                //printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit);
                                BMultiBodyJointLimitConstraint mbc = gameObject.AddComponent <BMultiBodyJointLimitConstraint>();
                                mbc.m_jointLowerLimit = jointLowerLimit;
                                mbc.m_jointUpperLimit = jointUpperLimit;
                            }
                            //printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
                        }
                        else
                        {
                            Generic6DofSpring2Constraint dof6 = creation.createPrismaticJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);

                            if (enableConstraints)
                            {
                                world1.AddConstraint(dof6, true);
                            }

                            //b3Printf("Prismatic\n");
                        }

                        break;
                    }

                    default:
                    {
                        //b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
                        Debug.Assert(false);
                        break;
                    }
                    }
                }

                if (createMultiBody)
                {
                    if (bmbl != null)
                    {
                        bmbl.jointDamping  = jointDamping;
                        bmbl.jointFriction = jointFriction;
                        //bmbl.jointLowerLimit = jointLowerLimit;
                        //bmbl.jointUpperLimit = jointUpperLimit;
                        //bmbl.jointMaxForce = jointMaxForce;
                        //bmbl.jointMaxVelocity = jointMaxVelocity;
                    }
                    {
                        /*
                         * MultiBodyLinkCollider col = creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
                         *
                         * compoundShape.GetCollisionShape().UserIndex = (graphicsIndex);
                         *
                         * col.CollisionShape = (compoundShape.GetCollisionShape());
                         *
                         * Matrix tr = Matrix.Identity;
                         *
                         * tr = linkTransformInWorldSpace;
                         * //if we don't set the initial pose of the btCollisionObject, the simulator will do this
                         * //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
                         *
                         * col.WorldTransform = (tr);
                         *
                         * //base and fixed? . static, otherwise flag as dynamic
                         * bool isDynamic = (mbLinkIndex < 0 && cache.m_bulletMultiBody.HasFixedBase) ? false : true;
                         * CollisionFilterGroups collisionFilterGroup = isDynamic ? (CollisionFilterGroups.DefaultFilter) : (CollisionFilterGroups.StaticFilter);
                         * CollisionFilterGroups collisionFilterMask = isDynamic ? (CollisionFilterGroups.AllFilter) : (CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter);
                         *
                         * CollisionFilterGroups colGroup = 0, colMask = 0;
                         * UrdfCollisionFlags collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex, out colGroup, out colMask);
                         * if ((collisionFlags & UrdfCollisionFlags.URDF_HAS_COLLISION_GROUP) != 0)
                         * {
                         *  collisionFilterGroup = colGroup;
                         * }
                         * if ((collisionFlags & UrdfCollisionFlags.URDF_HAS_COLLISION_MASK) != 0)
                         * {
                         *  collisionFilterMask = colMask;
                         * }
                         * world1.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
                         *
                         * color2 = Color.red;//(0.0,0.0,0.5);
                         * Color specularColor = new Color(1, 1, 1);
                         * UrdfMaterialColor matCol;
                         * if (u2b.getLinkColor2(urdfLinkIndex, out matCol))
                         * {
                         *  color2 = matCol.m_rgbaColor;
                         *  specularColor = matCol.m_specularColor;
                         * }
                         * {
                         *
                         *
                         *  creation.createCollisionObjectGraphicsInstance2(urdfLinkIndex, col, color2, specularColor);
                         * }
                         * {
                         *
                         *
                         *  u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, ref localInertialFrame, col, u2b.getBodyUniqueId());
                         * }
                         * URDFLinkContactInfo contactInfo;
                         * u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo);
                         *
                         *
                         * ProcessContactParameters(contactInfo, col);
                         *
                         * if (mbLinkIndex >= 0) //???? double-check +/- 1
                         * {
                         *  cache.m_bulletMultiBody.GetLink(mbLinkIndex).Collider = col;
                         *  if ((flags & ConvertURDFFlags.CUF_USE_SELF_COLLISION_EXCLUDE_PARENT) != 0)
                         *  {
                         *      cache.m_bulletMultiBody.GetLink(mbLinkIndex).Flags |= (int)btMultiBodyLinkFlags.BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
                         *  }
                         *  if ((flags & ConvertURDFFlags.CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) != 0)
                         *  {
                         *      cache.m_bulletMultiBody.GetLink(mbLinkIndex).Flags |= (int)btMultiBodyLinkFlags.BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION;
                         *  }
                         * }
                         * else
                         * {
                         *  cache.m_bulletMultiBody.BaseCollider = (col);
                         * }
                         */
                    }
                }
                else
                {
                    //u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape);
                }
            }


            List <int> urdfChildIndices = new List <int>();

            u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);

            int numChildren = urdfChildIndices.Count;

            for (int i = 0; i < numChildren; i++)
            {
                int urdfChildLinkIndex = urdfChildIndices[i];

                ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, gameObject, world1, createMultiBody, pathPrefix, enableConstraints, flags);
            }
        }