Ejemplo n.º 1
0
    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
             */
        }
    }