protected void OnInitializePhysics() { CollisionShapes = new List <CollisionShape>(); links = new List <MultiBodyLinkCollider>(); Debug.Log("Begin onInitialize physics"); //roboticists like Z up int upAxis = 2; //m_guiHelper.setUpAxis(upAxis); createEmptyDynamicsWorld(); BulletSharp.Math.Vector3 gravity = new BulletSharp.Math.Vector3(0, 0, 0); // gravity[upAxis]=-9.8; m_dynamicsWorld.Gravity = (gravity); Matrix baseWorldTrans = new Matrix(); baseWorldTrans.ScaleVector = BulletSharp.Math.Vector3.One; m_multiBody = createInvertedPendulumMultiBody(radius, m_dynamicsWorld, baseWorldTrans, false); if (true)//(m_multiBody != null) { // construct inverse model MultiBodyTreeCreator id_creator = new MultiBodyTreeCreator(); if (-1 == id_creator.CreateFromMultiBody(m_multiBody)) { Debug.LogError("error creating tree\n"); } else { m_inverseModel = id_creator.CreateMultiBodyTree(); } // add joint target controls qd = new float[m_multiBody.NumDofs]; qd_name = new string[m_multiBody.NumDofs]; q_name = new string[m_multiBody.NumDofs]; Debug.Log("Created inverse model"); } linkPositions = new UnityEngine.Vector3[links.Count]; Debug.Log("End onInitialize physics"); }
protected override void OnInitializePhysics() { Debug.Log("Begin onInitialize physics"); //roboticists like Z up int upAxis = 2; //m_guiHelper.setUpAxis(upAxis); createEmptyDynamicsWorld(); BulletSharp.Math.Vector3 gravity = new BulletSharp.Math.Vector3(0, 0, 0); // gravity[upAxis]=-9.8; m_dynamicsWorld.Gravity = (gravity); if (m_option == btInverseDynamicsExampleOptions.BT_ID_PROGRAMMATICALLY) { } switch (m_option) { case btInverseDynamicsExampleOptions.BT_ID_LOAD_URDF: { Debug.LogError("Not implemented"); break; } case btInverseDynamicsExampleOptions.BT_ID_PROGRAMMATICALLY: { Matrix baseWorldTrans = new Matrix(); baseWorldTrans.ScaleVector = BulletSharp.Math.Vector3.One; m_multiBody = createInvertedPendulumMultiBody(radius, m_dynamicsWorld, baseWorldTrans, false); break; } default: { Debug.LogError("Unknown option in initPhysics"); Debug.Assert(false); break; } } ; if (true)//(m_multiBody != null) { // construct inverse model MultiBodyTreeCreator id_creator = new MultiBodyTreeCreator(); if (-1 == id_creator.CreateFromMultiBody(m_multiBody)) { Debug.LogError("error creating tree\n"); } else { m_inverseModel = id_creator.CreateMultiBodyTree(); } // add joint target controls qd = new float[m_multiBody.NumDofs]; qd_name = new string[m_multiBody.NumDofs]; q_name = new string[m_multiBody.NumDofs]; Debug.Log("Created inverse model"); } Debug.Log("End onInitialize physics"); }
// Update is called once per frame void FixedUpdate() { MultiBody mb = m_cachedMultibody.GetMultiBody(); if (m_inverseModel == null) { // construct inverse model MultiBodyTreeCreator id_creator = new MultiBodyTreeCreator(); if (-1 == id_creator.CreateFromMultiBody(mb)) { Debug.LogError("error creating tree\n"); } else { m_inverseModel = id_creator.CreateMultiBodyTree(); } Debug.Log("Created inverse model"); } ReadCurrentPose(); GetDesiredPose(); CalculatedDesiredAccelerations(); int baseDofs = mb.HasFixedBase ? 0 : 6; int numDofs = mb.NumDofs; float[] qq, qqdot, ddesiredAcceleration; if (mb.HasFixedBase) { qq = q; qqdot = qdot; ddesiredAcceleration = desiredAcceleration; } else { // inverseModel stores dofs for the base if floating base. Need to prepad to handle this. qq = new float[baseDofs + numDofs]; qqdot = new float[baseDofs + numDofs]; ddesiredAcceleration = new float[baseDofs + numDofs]; for (int i = 0; i < numDofs; i++) { qq[i + baseDofs] = q[i]; qqdot[i + baseDofs] = qdot[i]; ddesiredAcceleration[i + baseDofs] = desiredAcceleration[i]; } } float[] joint_force = new float[numDofs + baseDofs]; if (-1 != m_inverseModel.CalculateInverseDynamics(mb.HasFixedBase, qq, qqdot, ddesiredAcceleration, joint_force)) { // use inverse model: apply joint force corresponding to // desired acceleration nu. for floating base will have been padded by baseDofs for (int dof = 0; dof < numDofs; dof++) { mb.AddJointTorque(dof, joint_force[dof + baseDofs]); } } else { Debug.LogError("Bad return from CalculateInverseDynamics"); } }