public bool OnBulletCreate() { if( rigidBodyObj != null ) // have created! { return true; } if( CollisionShapeObject == null ) // if user not give a collision, search it on itself first! CollisionShapeObject = GetComponent<BCollisionShape>(); if( CollisionShapeObject == null ) { Debug.LogError("Bullet RigidBody need a collision shape!"); return false; } bool cResult = CollisionShapeObject.OnBulletCreate(); if( cResult == false ) { Debug.LogError("Collision Shape Create Error!"); return false; } btTransform trans = new btTransform(); trans.setIdentity(); btVector3 pos = new btVector3(transform.position.x,transform.position.y,transform.position.z); trans.setOrigin(pos); trans.setRotation(new btQuaternion(transform.rotation.x,transform.rotation.y,transform.rotation.z,transform.rotation.w)); //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (Mass != 0.0f); btVector3 localInertia = new btVector3(0,0,0); if (isDynamic) { CollisionShapeObject.CalculateLocalInertia(Mass,localInertia); } //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects myMotionState = new btDefaultMotionState(trans); rbInfo = new btRigidBodyConstructionInfo(Mass,myMotionState.GetSwigPtr(),CollisionShapeObject.GetCollisionShapePtr(),localInertia.GetSwigPtr()); rigidBodyObj = new btRigidBody(rbInfo); collisionObject = btCollisionObject.GetObjectFromSwigPtr(rigidBodyObj.GetCollisionObject()); collisionObject.setFriction(Friction); return true; }
public int AddLinkToMultiBody(BMultiBody mb, int currentLinkIndex, int parentIndex, Transform parent) { if (isLinked) { Debug.LogErrorFormat("Cannot add link {0} to multibody {1} bacause it is already linked", name, mb.name); return(0); } BCollisionShape collisionShape = GetComponent <BCollisionShape>(); if (collisionShape == null) { throw new MissingComponentException("Could not find " + typeof(BCollisionShape).Name + " component on BodyLink " + name); } multiBody = mb; linkId = currentLinkIndex; parentLinkId = parentIndex; parentTransform = parent; CollisionShape shape = collisionShape.GetCollisionShape(); if (shape == null) { throw new MissingComponentException("Could not get collision shape from " + collisionShape.GetType().Name + " shape component on BodyLink " + name); } BulletSharp.Math.Vector3 linkInertia; shape.CalculateLocalInertia(Mass, out linkInertia); if (BPhysicsWorld.Get().debugType >= BulletUnity.Debugging.BDebug.DebugType.Debug) { Debug.LogFormat(this, "Adding link {0} : {1} to parent {2} of multibody {3}", currentLinkIndex, name, parentIndex, mb.name); } SetupLink(linkInertia); linkCollider = new MultiBodyLinkCollider(mb.MultiBody, currentLinkIndex); linkCollider.CollisionShape = shape; linkCollider.WorldTransform = transform.localToWorldMatrix.ToBullet(); linkCollider.CollisionFlags = collisionFlags; linkCollider.Friction = Friction; linkCollider.RollingFriction = RollingFriction; linkCollider.Restitution = Restitution; linkCollider.UserObject = this; BPhysicsWorld.Get().world.AddCollisionObject(linkCollider, groupsIBelongTo, collisionMask); m_collisionObject = linkCollider; BulletMultiBodyLinkColliderProxy proxy = gameObject.GetComponent <BulletMultiBodyLinkColliderProxy>(); if (proxy == null) { proxy = gameObject.AddComponent <BulletMultiBodyLinkColliderProxy>(); } mb.MultiBody.GetLink(currentLinkIndex).Collider = linkCollider; proxy.target = linkCollider; isLinked = true; foreach (BMultiBodyConstraint mbc in Constraints) { mbc.AddConstraintToMultiBody(MultiBody, LinkId); } int addedLinks = 1; for (int i = 0; i < Links.Count; ++i) { addedLinks += Links[i].AddLinkToMultiBody(mb, i + currentLinkIndex + 1, currentLinkIndex, transform); } return(addedLinks); }
//IMPORTANT Time.fixedTime must match the timestep being used here. public static List <UnityEngine.Vector3> SimulateBall(BRigidBody ballRb, UnityEngine.Vector3 ballThrowForce, int numberOfSimulationSteps, bool reverseOrder) { var ballPositions = new List <UnityEngine.Vector3>(numberOfSimulationSteps); //Create a World Debug.Log("Initialize physics"); CollisionConfiguration CollisionConf; CollisionDispatcher Dispatcher; BroadphaseInterface Broadphase; CollisionWorld cw; ConstraintSolver Solver; BulletSharp.SoftBody.SoftBodyWorldInfo softBodyWorldInfo; //This should create a copy of the BPhysicsWorld with the same settings BPhysicsWorld bw = BPhysicsWorld.Get(); bw.CreatePhysicsWorld(out cw, out CollisionConf, out Dispatcher, out Broadphase, out Solver, out softBodyWorldInfo); World = (DiscreteDynamicsWorld)cw; //Copy all existing rigidbodies in scene // IMPORTANT rigidbodies must be added to the offline world in the same order that they are in the source world // this is because collisions must be resolved in the same order for the sim to be deterministic DiscreteDynamicsWorld sourceWorld = (DiscreteDynamicsWorld)bw.world; BulletSharp.RigidBody bulletBallRb = null; BulletSharp.Math.Matrix mm = BulletSharp.Math.Matrix.Identity; for (int i = 0; i < sourceWorld.NumCollisionObjects; i++) { CollisionObject co = sourceWorld.CollisionObjectArray[i]; if (co != null && co.UserObject is BRigidBody) { BRigidBody rb = (BRigidBody)co.UserObject; float mass = rb.isDynamic() ? rb.mass : 0f; BCollisionShape existingShape = rb.GetComponent <BCollisionShape>(); CollisionShape shape = null; if (existingShape is BSphereShape) { shape = ((BSphereShape)existingShape).CopyCollisionShape(); } else if (existingShape is BBoxShape) { shape = ((BBoxShape)existingShape).CopyCollisionShape(); } RigidBody bulletRB = null; BulletSharp.Math.Vector3 localInertia = new BulletSharp.Math.Vector3(); rb.CreateOrConfigureRigidBody(ref bulletRB, ref localInertia, shape, null); BulletSharp.Math.Vector3 pos = rb.GetCollisionObject().WorldTransform.Origin; BulletSharp.Math.Quaternion rot = rb.GetCollisionObject().WorldTransform.GetOrientation(); BulletSharp.Math.Matrix.AffineTransformation(1f, ref rot, ref pos, out mm); bulletRB.WorldTransform = mm; World.AddRigidBody(bulletRB, rb.groupsIBelongTo, rb.collisionMask); if (rb == ballRb) { bulletBallRb = bulletRB; bulletRB.ApplyCentralImpulse(ballThrowForce.ToBullet()); } } } //Step the simulation numberOfSimulationSteps times for (int i = 0; i < numberOfSimulationSteps; i++) { int numSteps = World.StepSimulation(1f / 60f, 10, 1f / 60f); ballPositions.Add(bulletBallRb.WorldTransform.Origin.ToUnity()); } UnityEngine.Debug.Log("ExitPhysics"); if (World != null) { //remove/dispose constraints int i; for (i = World.NumConstraints - 1; i >= 0; i--) { TypedConstraint constraint = World.GetConstraint(i); World.RemoveConstraint(constraint); constraint.Dispose(); } //remove the rigidbodies from the dynamics world and delete them for (i = World.NumCollisionObjects - 1; i >= 0; i--) { CollisionObject obj = World.CollisionObjectArray[i]; RigidBody body = obj as RigidBody; if (body != null && body.MotionState != null) { body.MotionState.Dispose(); } World.RemoveCollisionObject(obj); obj.Dispose(); } World.Dispose(); Broadphase.Dispose(); Dispatcher.Dispose(); CollisionConf.Dispose(); } if (Broadphase != null) { Broadphase.Dispose(); } if (Dispatcher != null) { Dispatcher.Dispose(); } if (CollisionConf != null) { CollisionConf.Dispose(); } return(ballPositions); }
void OnDrawGizmos() { // draw itself // draw collision shape related to it. if( CollisionShapeObject == null ) { CollisionShapeObject = GetComponent<BCollisionShape>(); } // if collision shape and rigidbody in the same gameobject ,turn off collision debug draw. if( CollisionShapeObject != null ) { if( CollisionShapeObject.gameObject == gameObject ) CollisionShapeObject.SetDebugDraw(false); else CollisionShapeObject.SetDebugDraw(true); CollisionShapeObject.DebugDraw(transform.position,transform.rotation,transform.localScale,Color.red); } }
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); } }