void ReadCurrentPose()
{
MultiBody mb = m_cachedMultibody.GetMultiBody();
int numDofs = mb.NumDofs;
if (q == null || q.Length != numDofs)
{
q = new float[numDofs];
qdot = new float[numDofs];
desiredAcceleration = new float[numDofs];
pd_control = new float[numDofs];
desiredQ = new float[numDofs];
deisredQdot = new float[numDofs];
}
for (int dof = 0; dof < numDofs; dof++)
{
q[dof] = mb.GetJointPos(dof);
qdot[dof] = mb.GetJointVel(dof);
//Debug.Log("DOF " + dof + " is " + q[dof] * Mathf.Rad2Deg);
}
}
public override void OnUpdate()
{
if (true) //(m_multiBody != null)
{
int baseDofs = m_multiBody.HasFixedBase ? 0 : 6;
int num_dofs = m_multiBody.NumDofs;
float[] nu = new float[num_dofs + baseDofs];
float[] qdot = new float[num_dofs + baseDofs];
float[] q = new float[num_dofs + baseDofs];
float[] joint_force = new float[num_dofs + baseDofs];
float[] pd_control = new float[num_dofs + baseDofs];
// compute joint forces from one of two control laws:
// 1) "computed torque" control, which gives perfect, decoupled,
// linear second order error dynamics per dof in case of a
// perfect model and (and negligible time discretization effects)
// 2) decoupled PD control per joint, without a model
for (int dof = 0; dof < num_dofs; dof++)
{
q[dof] = m_multiBody.GetJointPos(dof);
qdot[dof] = m_multiBody.GetJointVel(dof);
float qd_dot = 0;
float qd_ddot = 0;
//if (m_timeSeriesCanvas)
// m_timeSeriesCanvas.insertDataAtCurrentTime(q[dof], dof, true);
// pd_control is either desired joint torque for pd control,
// or the feedback contribution to nu
pd_control[dof] = kd * (qd_dot - qdot[dof]) + kp * (qd[dof] - q[dof]);
// nu is the desired joint acceleration for computed torque control
nu[dof] = qd_ddot + pd_control[dof];
}
if (true)
{
// calculate joint forces corresponding to desired accelerations nu
if (m_multiBody.HasFixedBase)
{
Debug.Log("Adding body forces.");
for (int dof = 0; dof < num_dofs; dof++)
{
m_inverseModel.AddUserForce(dof, new BulletSharp.Math.Vector3(0, 1, 1));
}
if (-1 != m_inverseModel.CalculateInverseDynamics(m_multiBody.HasFixedBase, q, qdot, nu, joint_force))
{
//joint_force(dof) += damping*dot_q(dof);
// use inverse model: apply joint force corresponding to
// desired acceleration nu
for (int dof = 0; dof < num_dofs; dof++)
{
m_multiBody.AddJointTorque(dof, joint_force[dof]);
}
}
else
{
Debug.LogError("Bad return from CalculateInverseDynamics");
}
}
else
{
//the inverse dynamics model represents the 6 DOFs of the base, unlike btMultiBody.
//append some dummy values to represent the 6 DOFs of the base
float[] nu6 = new float[num_dofs + 6], qdot6 = new float[num_dofs + 6], q6 = new float[num_dofs + 6], joint_force6 = new float[num_dofs + 6];
for (int i = 0; i < num_dofs; i++)
{
nu6[6 + i] = nu[i];
qdot6[6 + i] = qdot[i];
q6[6 + i] = q[i];
joint_force6[6 + i] = joint_force[i];
}
if (-1 != m_inverseModel.CalculateInverseDynamics(m_multiBody.HasFixedBase, q6, qdot6, nu6, joint_force6))
{
//joint_force(dof) += damping*dot_q(dof);
// use inverse model: apply joint force corresponding to
// desired acceleration nu
for (int dof = 0; dof < num_dofs; dof++)
{
m_multiBody.AddJointTorque(dof, joint_force6[dof + 6]);
}
}
else
{
Debug.LogError("Bad return from CalculateInverseDynamics");
}
}
}
else
{
for (int dof = 0; dof < num_dofs; dof++)
{
// no model: just apply PD control law
m_multiBody.AddJointTorque(dof, pd_control[dof]);
}
}
}
// if (m_timeSeriesCanvas)
// m_timeSeriesCanvas.nextTick();
//todo: joint damping for btMultiBody, tune parameters
// step the simulation
if (m_dynamicsWorld != null)
{
// todo(thomas) check that this is correct:
// want to advance by 10ms, with 1ms timesteps
m_dynamicsWorld.StepSimulation(1e-3f, 0);//,1e-3);
/*
* btAlignedObjectArray<BulletSharp.Math.Quaternion> scratch_q;
* btAlignedObjectArray<BulletSharp.Math.Vector3> scratch_m;
* m_multiBody.ForwardKinematics(scratch_q, scratch_m);
*/
//"TODO forward kinematics";
/*
#if 0
* for (int i = 0; i < m_multiBody.getNumLinks(); i++)
* {
* //BulletSharp.Math.Vector3 pos = m_multiBody.getLink(i).m_cachedWorldTransform.getOrigin();
* btTransform tr = m_multiBody.getLink(i).m_cachedWorldTransform;
* BulletSharp.Math.Vector3 pos = tr.getOrigin() - quatRotate(tr.getRotation(), m_multiBody.getLink(i).m_dVector);
* BulletSharp.Math.Vector3 localAxis = m_multiBody.getLink(i).m_axes[0].m_topVec;
* //printf("link %d: %f,%f,%f, local axis:%f,%f,%f\n", i, pos.x(), pos.y(), pos.z(), localAxis.x(), localAxis.y(), localAxis.z());
* }
#endif
*/
}
}
public void OnUpdate()
{
if (true) //(m_multiBody != null)
{
int baseDofs = m_multiBody.HasFixedBase ? 0 : 6;
int num_dofs = m_multiBody.NumDofs;
float[] nu = new float[num_dofs + baseDofs];
float[] qdot = new float[num_dofs + baseDofs];
float[] q = new float[num_dofs + baseDofs];
float[] joint_force = new float[num_dofs + baseDofs];
float[] pd_control = new float[num_dofs + baseDofs];
// compute joint forces from one of two control laws:
// 1) "computed torque" control, which gives perfect, decoupled,
// linear second order error dynamics per dof in case of a
// perfect model and (and negligible time discretization effects)
// 2) decoupled PD control per joint, without a model
for (int dof = 0; dof < num_dofs; dof++)
{
q[dof] = m_multiBody.GetJointPos(dof);
qdot[dof] = m_multiBody.GetJointVel(dof);
float qd_dot = 0;
float qd_ddot = 0;
//if (m_timeSeriesCanvas)
// m_timeSeriesCanvas.insertDataAtCurrentTime(q[dof], dof, true);
// pd_control is either desired joint torque for pd control,
// or the feedback contribution to nu
pd_control[dof] = kd * (qd_dot - qdot[dof]) + kp * (qd[dof] - q[dof]);
// nu is the desired joint acceleration for computed torque control
nu[dof] = qd_ddot + pd_control[dof];
}
if (true)
{
// calculate joint forces corresponding to desired accelerations nu
if (m_multiBody.HasFixedBase)
{
Debug.Log("FixedBase Adding body forces.");
for (int dof = 0; dof < num_dofs; dof++)
{
m_inverseModel.AddUserForce(dof, new BulletSharp.Math.Vector3(0, 1, 1));
}
Debug.LogFormat("{0}, {1}, {2}, {3}, {4}", m_multiBody.HasFixedBase, q.Length, qdot.Length, nu.Length, joint_force.Length);
if (-1 != m_inverseModel.CalculateInverseDynamics(m_multiBody.HasFixedBase, q, qdot, nu, joint_force))
{
//joint_force(dof) += damping*dot_q(dof);
// use inverse model: apply joint force corresponding to
// desired acceleration nu
for (int dof = 0; dof < num_dofs; dof++)
{
m_multiBody.AddJointTorque(dof, joint_force[dof]);
}
}
else
{
Debug.LogError("Bad return from CalculateInverseDynamics");
}
}
else
{
Debug.Log("NotFixedBase Adding body forces.");
Debug.Log("Tree num bodies " + m_inverseModel.NumBodies);
Debug.Log("Tree num dofs " + m_inverseModel.NumDoFs);
Debug.Log("Tree num dofs " + m_inverseModel.GetAcceptInvalidMassProperties());
int bodyIdx = m_inverseModel.NumBodies - 2;
BulletSharp.Math.Vector3 v = new BulletSharp.Math.Vector3();
m_inverseModel.GetBodyAngularAcceleration(bodyIdx, out v);
Debug.Log("AngularAcceleration " + v);
m_inverseModel.GetBodyAngularVelocity(bodyIdx, out v);
Debug.Log("Ang Vel " + v);
m_inverseModel.GetBodyAxisOfMotion(bodyIdx, out v);
Debug.Log("AxisOfMotion " + v);
m_inverseModel.GetBodyCoM(bodyIdx, out v);
Debug.Log("CoM " + v);
m_inverseModel.GetBodyDotJacobianRotU(bodyIdx, out v);
Debug.Log("GetBodyDotJacobianRotU " + v);
m_inverseModel.GetBodyDotJacobianTransU(bodyIdx, out v);
Debug.Log("GetBodyDotJacobianTransU " + v);
m_inverseModel.GetBodyFirstMassMoment(bodyIdx, out v);
Debug.Log("GetBodyFirstMassMoment " + v);
m_inverseModel.GetBodyLinearAcceleration(bodyIdx, out v);
Debug.Log("GetBodyLinearAcceleration " + v);
m_inverseModel.GetBodyLinearVelocity(bodyIdx, out v);
Debug.Log("GetBodyLinearVelocity " + v);
m_inverseModel.GetBodyLinearVelocityCoM(bodyIdx, out v);
Debug.Log("GetBodyLinearVelocityCoM " + v);
float m = 0;
m_inverseModel.GetBodyMass(bodyIdx, out m);
Debug.Log("GetBodyMass " + m);
m_inverseModel.GetBodyOrigin(bodyIdx, out v);
Debug.Log("GetBodyOrigin " + v);
Matrix3x3FloatData fd;
m_inverseModel.GetBodySecondMassMoment(bodyIdx, out fd);
Debug.Log("GetBodySecondMassMoment " + fd);
m_inverseModel.GetBodyTParentRef(bodyIdx, out fd);
Debug.Log("GetBodyTParentRef " + fd);
m_inverseModel.GetBodyTransform(bodyIdx, out fd);
Debug.Log("GetBodyTransform " + fd);
int dofOffset;
m_inverseModel.GetDoFOffset(bodyIdx, out dofOffset);
Debug.Log("GetDoFOffset " + dofOffset);
int pIdx;
m_inverseModel.GetParentIndex(bodyIdx, out pIdx);
Debug.Log("GetParentIndex" + pIdx);
m_inverseModel.GetParentRParentBodyRef(bodyIdx, out v);
Debug.Log("GetParentRParentBodyRef" + v);
int uIdx;
m_inverseModel.GetUserInt(bodyIdx, out uIdx);
Debug.Log("GetUserInt " + uIdx);
bool acceptInvalidMassParams = true;
m_inverseModel.SetAcceptInvalidMassParameters(acceptInvalidMassParams);
Debug.Assert(m_inverseModel.GetAcceptInvalidMassProperties() == acceptInvalidMassParams);
v = new BulletSharp.Math.Vector3(3, 0, 0);
BulletSharp.Math.Vector3 vv;
m_inverseModel.SetBodyFirstMassMoment(bodyIdx, ref v);
m_inverseModel.GetBodyFirstMassMoment(bodyIdx, out vv);
Debug.Assert(v.Equals(vv));
float bMass;
m_inverseModel.SetBodyMass(bodyIdx, 4);
m_inverseModel.GetBodyMass(bodyIdx, out bMass);
Debug.Assert(bMass == 4, " body mass " + bMass);
v = new BulletSharp.Math.Vector3(.1f, -10f, .1f);
m_inverseModel.SetGravityInWorldFrame(v);
JointType jt;
m_inverseModel.GetJointType(bodyIdx, out jt);
Debug.Log("JointType " + jt);
m_inverseModel.GetBodySecondMassMoment(bodyIdx, out fd);
m_inverseModel.SetBodySecondMassMoment(bodyIdx, ref fd);
float[] jacobianTrans = new float[3 * num_dofs];
m_inverseModel.GetBodyJacobianTrans(bodyIdx, num_dofs, 6, jacobianTrans);
float[] jacobianRot = new float[3 * num_dofs];
m_inverseModel.GetBodyJacobianRot(bodyIdx, num_dofs, 6, jacobianRot);
//=====================================
m_inverseModel.ClearAllUserForcesAndMoments();
for (int dof = 0; dof < num_dofs; dof++)
{
m_inverseModel.AddUserForce(dof, new BulletSharp.Math.Vector3(0, 1, 1));
m_inverseModel.AddUserMoment(dof, new BulletSharp.Math.Vector3(1, 0, 1));
}
//the inverse dynamics model represents the 6 DOFs of the base, unlike btMultiBody.
//append some dummy values to represent the 6 DOFs of the base
float[] nu6 = new float[num_dofs + 6], qdot6 = new float[num_dofs + 6], q6 = new float[num_dofs + 6], joint_force6 = new float[num_dofs + 6];
for (int i = 0; i < num_dofs; i++)
{
nu6[6 + i] = nu[i];
qdot6[6 + i] = qdot[i];
q6[6 + i] = q[i];
joint_force6[6 + i] = joint_force[i];
}
Debug.LogFormat("{0}, {1}, {2}, {3}, {4}, {5}", m_multiBody.HasFixedBase, q6.Length, qdot6.Length, nu6.Length, joint_force6.Length, num_dofs);
if (-1 != m_inverseModel.CalculateInverseDynamics(m_multiBody.HasFixedBase, q6, qdot6, nu6, joint_force6))
{
//joint_force(dof) += damping*dot_q(dof);
// use inverse model: apply joint force corresponding to
// desired acceleration nu
for (int dof = 0; dof < num_dofs; dof++)
{
m_multiBody.AddJointTorque(dof, joint_force6[dof + 6]);
}
}
else
{
Debug.LogError("Bad return from CalculateInverseDynamics");
}
float[] massMatrix = new float[num_dofs * num_dofs];
if (-1 == m_inverseModel.CalculateMassMatrix(m_multiBody.HasFixedBase, q6, massMatrix))
{
Debug.LogError("Bad return from CalculateMassMatrix");
}
if (-1 == m_inverseModel.CalculateMassMatrix(m_multiBody.HasFixedBase, q6, true, true, true, massMatrix))
{
Debug.LogError("Bad return from CalculateMassMatrix");
}
if (-1 == m_inverseModel.CalculateJacobians(m_multiBody.HasFixedBase, q6))
{
Debug.LogError("Bad return from CalculateJacobians");
}
if (-1 == m_inverseModel.CalculateJacobians(m_multiBody.HasFixedBase, q6, qdot6))
{
Debug.LogError("Bad return from CalculateJacobians");
}
if (-1 == m_inverseModel.CalculateKinematics(m_multiBody.HasFixedBase, q6, qdot6, nu6))
{
Debug.LogError("Bad return from CalculateKinematics");
}
if (-1 == m_inverseModel.CalculatePositionAndVelocityKinematics(m_multiBody.HasFixedBase, q6, qdot6))
{
Debug.LogError("Bad return from CalculatePositionAndVelocityKinematics");
}
if (-1 == m_inverseModel.CalculatePositionKinematics(m_multiBody.HasFixedBase, q6))
{
Debug.LogError("Bad return from CalculatePositionKinematics");
}
}
}
else
{
for (int dof = 0; dof < num_dofs; dof++)
{
// no model: just apply PD control law
m_multiBody.AddJointTorque(dof, pd_control[dof]);
}
}
}
// if (m_timeSeriesCanvas)
// m_timeSeriesCanvas.nextTick();
//todo: joint damping for btMultiBody, tune parameters
// step the simulation
if (m_dynamicsWorld != null)
{
// todo(thomas) check that this is correct:
// want to advance by 10ms, with 1ms timesteps
m_dynamicsWorld.StepSimulation(1e-3f, 0);//,1e-3);
/*
* btAlignedObjectArray<BulletSharp.Math.Quaternion> scratch_q;
* btAlignedObjectArray<BulletSharp.Math.Vector3> scratch_m;
* m_multiBody.ForwardKinematics(scratch_q, scratch_m);
*/
//"TODO forward kinematics";
/*
#if 0
* for (int i = 0; i < m_multiBody.getNumLinks(); i++)
* {
* //BulletSharp.Math.Vector3 pos = m_multiBody.getLink(i).m_cachedWorldTransform.getOrigin();
* btTransform tr = m_multiBody.getLink(i).m_cachedWorldTransform;
* BulletSharp.Math.Vector3 pos = tr.getOrigin() - quatRotate(tr.getRotation(), m_multiBody.getLink(i).m_dVector);
* BulletSharp.Math.Vector3 localAxis = m_multiBody.getLink(i).m_axes[0].m_topVec;
* //printf("link %d: %f,%f,%f, local axis:%f,%f,%f\n", i, pos.x(), pos.y(), pos.z(), localAxis.x(), localAxis.y(), localAxis.z());
* }
#endif
*/
}
}
