Exemplo n.º 1
0
        public override void HandleMessage(string message)
        {
            MTMState state = JsonUtility.FromJson <MTMState>(message);

            if (!messageFirstParsed)
            {
                if (!CheckConsistency(state))
                {
                    messageFirstParsed = false;
                    return;
                }
                else
                {
                    messageFirstParsed = true;
                }
            }
            int currentIndex = 0;

            // Assuming correct order
            foreach (URDFJoint joint in independentJoints)
            {
                if (joint.jointType == URDFJoint.JointType.Prismatic)
                {
                    joint.SetJointValue(state.GetStateJoint.Position[currentIndex]);
                }
                else
                {
                    joint.SetJointValue(state.GetStateJoint.Position[currentIndex] / (float)(Math.PI) * 180f);
                }
                currentIndex++;
            }
        }
Exemplo n.º 2
0
        private bool CheckConsistency(MTMState state)
        {
            int currentIndex = 0;

            foreach (URDFJoint joint in independentJoints)
            {
                if (joint.name.StartsWith(state.GetStateJoint.Name[currentIndex]))
                {
                    currentIndex++;
                    continue;
                }
                else
                {
                    Debug.Log("MTM error: " + joint.name + " does not start with " + state.GetStateJoint.Name[currentIndex]);
                    return(false);
                }
            }
            Debug.Log("MTM consistency check passed");
            return(true);
        }