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