Inheritance: MonoBehaviour, IOptimizable
Example #1
0
    void drawStepCycles(float p_phi, float p_yOffset, LegFrame p_frame, int legFrameId)
    {
        for (int i = 0; i < LegFrame.c_legCount; i++)
        {
            StepCycle cycle = p_frame.m_tuneStepCycles[i];
            if (cycle != null)
            {
                // DRAW!
                float timelineLen = 300.0f;
                float xpad        = 10.0f;
                float offset      = cycle.m_tuneStepTrigger;
                float len         = cycle.m_tuneDutyFactor;
                float lineStart   = xpad;
                float lineEnd     = lineStart + timelineLen;
                float dutyEnd     = lineStart + timelineLen * (offset + len);
                float w           = 4.0f;
                float y           = p_yOffset + (float)i * w * 2.0f;
                bool  stance      = cycle.isInStance(p_phi);
                // Draw back
                Color ucol = Color.white * 0.5f + new Color((float)(legFrameId % 2), (float)(i % 2), 1 - (float)(i % 2), 1.0f);
                int   h    = (int)w / 2;
                Drawing.DrawLine(new Vector2(lineStart - 1, y - h - 1), new Vector2(lineEnd + 1, y - h - 1), Color.black, 1);
                Drawing.DrawLine(new Vector2(lineStart - 1, y + h), new Vector2(lineEnd + 1, y + h), Color.black, 1);
                Drawing.DrawLine(new Vector2(lineStart - 1, y - h - 1), new Vector2(lineStart - 1, y + h + 1), Color.black, 1);
                Drawing.DrawLine(new Vector2(lineEnd + 1, y - h - 1), new Vector2(lineEnd + 1, y + h), Color.black, 1);
                Drawing.DrawLine(new Vector2(lineStart, y), new Vector2(lineEnd, y), new Color(1.0f, 1.0f, 1.0f, 1.0f), w);
                // Color depending on stance
                Color currentCol = Color.black;
                float phase      = cycle.getStancePhase(p_phi);
                if (stance)
                {
                    currentCol = Color.Lerp(ucol, Color.black, phase * phase);
                }

                // draw df
                Drawing.DrawLine(new Vector2(lineStart + timelineLen * offset, y), new Vector2(Mathf.Min(lineEnd, dutyEnd), y), currentCol, w);
                // draw rest if out of bounds
                if (offset + len > 1.0f)
                {
                    Drawing.DrawLine(new Vector2(lineStart, y), new Vector2(lineStart + timelineLen * (offset + len - 1.0f), y), currentCol, w);
                }

                // Draw current time marker
                Drawing.DrawLine(new Vector2(lineStart + timelineLen * p_phi - 1, y), new Vector2(lineStart + timelineLen * p_phi + 3, y),
                                 Color.red, w);
                Drawing.DrawLine(new Vector2(lineStart + timelineLen * p_phi, y), new Vector2(lineStart + timelineLen * p_phi + 2, y),
                                 Color.green * 2, w);
            }
        }
    }
Example #2
0
    // Quick and dirty temporary solution for applying
    // foot torque. This should be done to the global torque array
    // in the final solution
    public void tempApplyFootTorque(float p_phi)
    {
        /*
         * Toe-off is modeled with a linear target trajectory towards a fixed toe-off
         * target angle θa that is triggered ∆ta seconds in advance of the start of the swing phase,
         * as dictated by the gait graph.
         *
         * Foot- strike anticipation is done in an analogous fashion with
         * respect to the anticipated foot-strike time and defined by θb and ∆tb.
         */
        for (int i = 0; i < c_legCount; i++)
        {
            StepCycle stepcycle         = m_tuneStepCycles[i];
            Transform foot              = m_feet[i].transform;
            float toeOffStartOffset     = m_tuneToeOffTime[i];
            float footStrikeStartOffset = m_tuneFootStrikeTime[i];
            // get absolutes
            // Get point in time when toe lifts off from ground
            float toeOffStart = stepcycle.m_tuneStepTrigger + stepcycle.m_tuneDutyFactor - toeOffStartOffset;
            if (toeOffStart >= 1.0f)
            {
                toeOffStart -= 1.0f;
            }
            if (toeOffStart < 0.0f)
            {
                toeOffStart += 1.0f;
            }
            // Get point in time when foot touches the ground
            float footStrikeStart = stepcycle.m_tuneStepTrigger - footStrikeStartOffset;
            if (toeOffStart < 0.0f)
            {
                footStrikeStart += 1.0f;
            }
            //
            Vector3 torque = Vector3.zero; Color rot = Color.red;
            if ((p_phi > footStrikeStart && footStrikeStart > toeOffStart) || p_phi < toeOffStart) // catch first
            {
                torque = Vector3.right * (foot.localRotation.eulerAngles.x - m_tuneFootStrikeAngle);
            }
            else/* if (p_phi < toeOffStart)*/
            {
                torque = Vector3.right * (foot.localRotation.eulerAngles.x - m_tuneToeOffAngle);
                rot    = Color.green;
            }
            foot.rigidbody.AddRelativeTorque(torque /**Time.deltaTime*/);

            foot.renderer.material.color = rot * Mathf.Abs(torque.x);
        }
    }
Example #3
0
 void debugColorLegs()
 {
     for (int i = 0; i < m_legFrames.Length; i++)
     {
         LegFrame lf = m_legFrames[i];
         for (int n = 0; n < lf.m_tuneStepCycles.Length; n++)
         {
             StepCycle cycle   = lf.m_tuneStepCycles[n];
             Rigidbody current = m_joints[lf.m_neighbourJointIds[n]];
             if (lf.isInControlledStance(n, m_player.m_gaitPhase))
             {
                 current.gameObject.GetComponentInChildren <Renderer>().material.color = Color.yellow;
             }
             else
             {
                 current.gameObject.GetComponentInChildren <Renderer>().material.color = Color.white;
             }
         }
     }
 }
Example #4
0
 // Compute the torque of all PD-controllers in the joints
 Vector3[] computePDTorques(float p_phi)
 {
     // This loop might have to be rewritten into something a little less cumbersome
     Vector3[] newTorques = new Vector3[m_jointTorques.Length];
     if (m_usePDTorque)
     {
         for (int i = 0; i < m_legFrames.Length; i++)
         {
             LegFrame lf = m_legFrames[i];
             newTorques[lf.m_id] = m_jointTorques[lf.m_id];
             // All hip joints
             for (int n = 0; n < lf.m_tuneStepCycles.Length; n++)
             {
                 StepCycle cycle   = lf.m_tuneStepCycles[n];
                 int       jointID = lf.m_neighbourJointIds[n];
                 if (lf.isInControlledStance(i, m_player.m_gaitPhase))
                 {
                     newTorques[jointID] = Vector3.zero;
                     // m_jointTorques[jointID];
                     //Vector3.zero;
                     //
                 }
                 else if (m_desiredJointTorquesPD.Length > 0)
                 {
                     newTorques[jointID] = m_desiredJointTorquesPD[jointID].m_vec;
                 }
             }
             // All other joints
             for (int n = 0; n < lf.m_legJointIds.Length; n++)
             {
                 int jointID = lf.m_legJointIds[n];
                 if (jointID > -1)
                 {
                     newTorques[jointID] = m_desiredJointTorquesPD[jointID].m_vec;
                 }
             }
         }
     }
     return(newTorques);
 }