// Deep copy constructor
        public MotionFrame(MotionFrame mf, bool isdeep = true)
        {
            this.BehaviorName = mf.BehaviorName;
            this.PoseID = mf.PoseID;
            this.IsAbsolute = mf.IsAbsolute;
            this.HoldTime = mf.HoldTime;
            this.TimeStamp = mf.TimeStamp;
            this.UsingSpeedFraction = mf.UsingSpeedFraction;
            this.SpeedFraction = mf.SpeedFraction;
            this.SpeedTime = mf.SpeedTime;

            if (isdeep==true)
            {
                if (mf.PoseList!=null)
                {
                    this.PoseList = new List<PoseProfile>();
                    foreach (var pp in mf.PoseList)
                    {
                        // Deep copy
                        var newpp = new PoseProfile(pp);
                        this.PoseList.Add(newpp);
                    }
                }
                if (mf.ComboJointNames!=null)
                {
                    this.ComboJointNames = new List<string>();
                    foreach (var n in mf.ComboJointNames)
                    {
                        this.ComboJointNames.Add(n);
                    }
                }
                if (mf.ComboJointValueDegree!=null)
                {
                    this.ComboJointValueDegree = new List<float>();
                    foreach (var jvd in mf.ComboJointValueDegree)
                    {
                        this.ComboJointValueDegree.Add(jvd);
                    }
                }
                if (mf.ComboJointValueRadian!=null)
                {
                    this.ComboJointValueRadian = new List<float>();
                    foreach (var jvr in mf.ComboJointValueRadian)
                    {
                        this.ComboJointValueRadian.Add(jvr);
                    }
                }
                if (mf.ComboJointSpeedT!=null)
                {
                    this.ComboJointSpeedT = new List<float>();
                    foreach (var jst in mf.ComboJointSpeedT)
                    {
                        this.ComboJointSpeedT.Add(jst);
                    }
                }
            }
        }
Пример #2
0
        /// <summary>
        /// Keep the head looking forward. 
        /// Disable all head manipulation.
        /// </summary>
        private void HeadInhibition(MotionFrame mf)
        {
            if (HeadYawInhibition == true)
            {
                int indhy = mf.ComboJointNames.IndexOf("HeadYaw");
                if (indhy >= 0)
                {
	                mf.ComboJointNames.RemoveAt(indhy);
	                mf.ComboJointValueRadian.RemoveAt(indhy);
                }
            }

            if (HeadPitchInhibition == true)
            {
                int indhp = mf.ComboJointNames.IndexOf("HeadPitch");
                if (indhp >= 0)
                {
	                mf.ComboJointNames.RemoveAt(indhp);
	                mf.ComboJointValueRadian.RemoveAt(indhp);
                }
            }
        }
Пример #3
0
        /// <summary>
        /// 
        /// </summary>
        /// <param name="mf"></param>
        void threadAngleInterpolation(MotionFrame mf)
        {
            // Mutex
            lock (lockMotion)
            {
                HeadInhibition(mf);

                try
                {
                    if (mf.UsingSpeedFraction == false) 
                    {
                        List<float> joint_times = SmoothMoveTime(mf.ComboJointNames, mf.ComboJointValueRadian, (float)mf.SpeedFraction);
                        proxyMotion.angleInterpolation(mf.ComboJointNames, mf.ComboJointValueRadian, joint_times, mf.IsAbsolute);
                    }
                    else //mf.UsingSpeedFraction == true
                    {

                        List<float> jv = mf.ComboJointValueRadian;
                        proxyMotion.angleInterpolationWithSpeed(mf.ComboJointNames, mf.ComboJointValueRadian, (float)mf.SpeedFraction);
                    }
                }
                catch (System.Exception ex)
                {
                    Debug.WriteLine("MotionProxy.angleInterpolation Exception: " + ex);
                }
            }
        }
        // motion parameters
        protected override MotionTimeline CoreParameterToMotion(bool right = true)
        {
            double pointingspeed = Parameters["MotionSpeed"].Value;
            double decayspeed = Parameters["DecaySpeed"].Value;
            double holdtime = Parameters["HoldTime"].Value;
            double repetition = Parameters["Repetition"].Value;

            // end pose parameters
            List<double> jointVals = CoreParameterToJoint();
            double shoulderPitch = jointVals[0];
            double shoulderRoll = jointVals[1];
            double elbowYaw = jointVals[2];
            double elbowRoll = jointVals[3];
            double wristYaw = jointVals[4];
            double hand = jointVals[5];
            double headpitch = jointVals[6];
            double headyaw = jointVals[7];

            //////////////////////////////////////////////////////////////////////////
            // initial pose that prepares to point
            double initHoldTime = 0.5;
            //double initSpeed = 0.3;
            // pose
            double shoulderPitch_1 = 20;
            double shoulderRoll_1 = -10;
            double elbowYaw_1 = 60;
            double elbowRoll_1 = 88;
            double wristYaw_1 = -20;
            double hand_1 = 0.6; // *180 / (double)Math.PI;
            //double headpitch_1 = 0;
            //double headyaw_1 = 0;
            //////////////////////////////////////////////////////////////////////////

            // Initial Frame
            PoseProfile initArmPose = new PoseProfile();
            initArmPose.Joints = new RArm(false,
                    shoulderPitch_1,
                    shoulderRoll_1,
                    elbowYaw_1,
                    elbowRoll_1,
                    wristYaw_1,
                    hand_1);
            PoseProfile initHeadPose = new PoseProfile();
            //initHeadPose.Joints = new Head(false, headpitch_1, headyaw_1);
            initHeadPose.Joints = new Head(false, headpitch, headyaw);
            MotionFrame initMF = new MotionFrame();
            initMF.PoseList.Add(initArmPose);
            initMF.PoseList.Add(initHeadPose);
            //initMF.SpeedFraction = initSpeed;
            initMF.SpeedFraction = pointingspeed;
            initMF.UsingSpeedFraction = true;
            initMF.HoldTime = initHoldTime;
            initMF.IsAbsolute = true;
            initMF.JointsToList();
            // End Frame (pointing)
            PoseProfile endArmPose = new PoseProfile();
            endArmPose.Joints = new RArm(false,
                    shoulderPitch,
                    shoulderRoll,
                    elbowYaw,
                    elbowRoll,
                    wristYaw,
                    hand);
            //PoseProfile endHeadPose = new PoseProfile();
            //endHeadPose.Joints = new Head(false, headpitch, headyaw);
            MotionFrame endMF = new MotionFrame();
            endMF.PoseList.Add(endArmPose);
            //endMF.PoseList.Add(endHeadPose);
            endMF.SpeedFraction = pointingspeed;
            endMF.UsingSpeedFraction = true;
            endMF.HoldTime = holdtime;
            endMF.IsAbsolute = true;
            endMF.JointsToList();

            // Repeat Frame (pointing)
            MotionFrame repMF = new MotionFrame();
            if (repetition > 0)
            {
                double repHoldTime = 0.0;
                //////////////////////////////////////////////////////////////////////////
                // Pose for Repeat
                /*double shoulderPitch_2, shoulderRoll_2, elbowRoll_2, elbowYaw_2, wristYaw_2, Hand_2;
                shoulderPitch_2 = 0;
                shoulderRoll_2 = 0;
                elbowYaw_2 = 80;
                elbowRoll_2 = 80;
                wristYaw_2 = 80;
                Hand_2 = 0.6 * 180 / (double)Math.PI;*/

                double percent = 0.5;
                double shoulderPitch_2 = Interpolation(shoulderPitch_1, shoulderPitch, percent);
                //double shoulderPitch_2 = shoulderPitch;
                double shoulderRoll_2 = Interpolation(shoulderRoll_1, shoulderRoll, percent);
                //double shoulderRoll_2 = shoulderRoll;
                double elbowYaw_2 = Interpolation(elbowYaw_1, elbowYaw, percent);
                //double elbowYaw_2 = elbowYaw;
                double elbowRoll_2 = Interpolation(elbowRoll_1, elbowRoll, percent);
                double wristYaw_2 = Interpolation(wristYaw_1, wristYaw, percent);
                //double wristYaw_2 = wristYaw;
                double Hand_2 = Interpolation(hand_1, hand, percent);
                //double Hand_2 = hand;

                /*
                PointingParams pp1 = new PointingParams(pp);
                pp1.Amplitude -= 0.1;
                List<double> jointVals_2 = PointingJoints(pp1);
                double shoulderPitch_2 = jointVals_2[0];
                double shoulderRoll_2 = jointVals_2[1];
                double elbowYaw_2 = jointVals_2[2];
                double elbowRoll_2 = jointVals_2[3];
                double wristYaw_2 = jointVals_2[4];
                double Hand_2 = jointVals_2[5];
                */
                //////////////////////////////////////////////////////////////////////////
                PoseProfile repArmPose = new PoseProfile();
                repArmPose.Joints = new RArm(false,
                        shoulderPitch_2,
                        shoulderRoll_2,
                        elbowYaw_2,
                        elbowRoll_2,
                        wristYaw_2,
                        Hand_2);
                //PoseProfile repHeadPose = new PoseProfile();
                //repHeadPose.Joints = new Head(false, headpitch, headyaw);
                repMF.PoseList.Add(repArmPose);
                //repMF.PoseList.Add(repHeadPose);
                repMF.SpeedFraction = pointingspeed;
                repMF.UsingSpeedFraction = true;
                repMF.HoldTime = repHoldTime;
                repMF.IsAbsolute = true;
                repMF.JointsToList();
            }

            // Time Line
            MotionTimeline mtl = new MotionTimeline();
            mtl.BehaviorsDesp = "Pointing";
            mtl.OrderedMFSeq.Add(initMF);
            //
            MotionFrame endMF1 = new MotionFrame(endMF);
            endMF1.HoldTime = 0;
            // Repetition
            for (int i = 0; i < repetition; i++)
            {
                mtl.OrderedMFSeq.Add(endMF1);
                mtl.OrderedMFSeq.Add(repMF);
            }
            mtl.OrderedMFSeq.Add(endMF);

            // decay
            mtl.PoseRecover = true;
            mtl.RecovSpd = decayspeed;
            return mtl;
        }
        /// <summary>
        /// Behavior parameters -> MotionTimeLine
        ///   1. Initial pose
        ///   2. End pose
        /// </summary>
        /// <param name="behparams"></param>
        /// <param name="pose">TL,TR,BL,BR</param>
        /// <returns></returns>
        protected override MotionTimeline CoreParameterToMotion(bool right = true)
        {
            double motionspeed = Parameters["MotionSpeed"].Value;
            double holdtime = Parameters["HoldTime"].Value;

            List<double> jointvalues = CoreParameterToJoint(right);

            double shoulderPitch  = jointvalues[0];
            double shoulderRoll   = jointvalues[1];
            double elbowYaw       = jointvalues[2];
            double elbowRoll      = jointvalues[3];
            double wristYaw       = jointvalues[4];
            double hand           = jointvalues[5];
            double headpitch      = jointvalues[6];
            double headyaw        = jointvalues[7];

            //////////////////////////////////////////////////////////////////////////
            // Accompanying
            PoseProfile HeadPose = new PoseProfile();
            HeadPose.Joints = new Head(false, headpitch, headyaw);
            //////////////////////////////////////////////////////////////////////////

            //////////////////////////////////////////////////////////////////////////
            // initial pose that prepares to point
            double initHoldTime = 0.1;
            //double initSpeed = 0.3;
            // pose
            double shoulderPitch_1 = 70;
            double shoulderRoll_1 = -10;
            double elbowYaw_1 = 45;
            double elbowRoll_1 = 88;
            double wristYaw_1 = -20;
            double hand_1 = 0.6;
            //double headpitch_1 = 0;
            //double headyaw_1 = 0;
            //////////////////////////////////////////////////////////////////////////

            // Initial Frame
            PoseProfile initArmPose = new PoseProfile();
            if (right == true) // right
            {
	            initArmPose.Joints = new RArm(false,
	                    shoulderPitch_1,
	                    shoulderRoll_1,
	                    elbowYaw_1,
	                    elbowRoll_1,
	                    wristYaw_1,
	                    hand_1);
            } 
            else // left
            {
                initArmPose.Joints = new LArm(false,
                        shoulderPitch_1,
                        -shoulderRoll_1,
                        -elbowYaw_1,
                        -elbowRoll_1,
                        -wristYaw_1,
                        hand_1);
            }
            PoseProfile initHeadPose = new PoseProfile();
            //initHeadPose.Joints = new Head(false, headpitch_1, headyaw_1);
            initHeadPose.Joints = new Head(false, headpitch, headyaw);
            MotionFrame initMF = new MotionFrame();
            initMF.PoseList.Add(initArmPose);
            initMF.PoseList.Add(initHeadPose);
            //initMF.SpeedFraction = initSpeed;
            initMF.SpeedFraction = motionspeed;
            initMF.UsingSpeedFraction = true;
            initMF.HoldTime = initHoldTime;
            initMF.IsAbsolute = true;
            initMF.JointsToList();
            // End Frame (pointing)
            PoseProfile endArmPose = new PoseProfile();
            if (right == true) // right
            {
	            endArmPose.Joints = new RArm(false,
	                    shoulderPitch,
	                    shoulderRoll,
	                    elbowYaw,
	                    elbowRoll,
	                    wristYaw,
	                    hand);
            } 
            else // left
            {
                endArmPose.Joints = new LArm(false,
                    shoulderPitch,
                    shoulderRoll,
                    elbowYaw,
                    elbowRoll,
                    wristYaw,
                    hand);
            }
            //PoseProfile endHeadPose = new PoseProfile();
            //endHeadPose.Joints = new Head(false, headpitch, headyaw);
            MotionFrame endMF = new MotionFrame();
            endMF.PoseList.Add(endArmPose);
            //endMF.PoseList.Add(endHeadPose);
            endMF.SpeedFraction = motionspeed;
            endMF.UsingSpeedFraction = true;
            endMF.HoldTime = holdtime;
            endMF.IsAbsolute = true;
            endMF.JointsToList();

            // Time Line
            MotionTimeline mtl = new MotionTimeline();
            mtl.BehaviorsDesp = "IGBehavior";
            mtl.OrderedMFSeq.Add(initMF);
            mtl.OrderedMFSeq.Add(endMF);

            // decay
            mtl.PoseRecover = true;
            mtl.RecovSpd = motionspeed - 0.03D;
            if (mtl.RecovSpd < 0.02) mtl.RecovSpd = 0.02;
            return mtl;
        }
Пример #6
0
        /// <summary>
        /// 
        /// </summary>
        /// <param name="joints"></param>
        /// <returns></returns>
        public MotionTimeline MirrorLeftRight()
        {
            MotionTimeline mtl = new MotionTimeline();
            mtl.BehaviorsDesp  = this.BehaviorsDesp;
            mtl.PoseRecover    = this.PoseRecover;
            mtl.RecovSpd       = this.RecovSpd;      

            // do not use "foreach". Keep the same order.
            for (int i = 0; i < this.OrderedMFSeq.Count; i++ )
            {
                MotionFrame mf = this.OrderedMFSeq[i];
                MotionFrame newmf = new MotionFrame();
                newmf.BehaviorName = mf.BehaviorName;
                newmf.IsAbsolute = mf.IsAbsolute;
                newmf.UsingSpeedFraction = mf.UsingSpeedFraction;
                newmf.SpeedFraction = mf.SpeedFraction;
                newmf.SpeedTime = mf.SpeedTime;
                newmf.TimeStamp = mf.TimeStamp;
                newmf.HoldTime = mf.HoldTime;
                foreach (PoseProfile pp in mf.PoseList)
                {
                    // shallow copy
                    PoseProfile newpp = new PoseProfile(pp,false);

                    JointChain jc = pp.Joints.MirrorLeftRight();
                    newpp.Joints = jc;

                    newmf.PoseList.Add(newpp);
                }
                // this must be called
                newmf.JointsToList();
                // keep the same order
                mtl.OrderedMFSeq.Add(newmf);
            }

            return mtl;
        }
Пример #7
0
        /// <summary>
        /// Internal use!
        /// Merge motion frames from different behavior profiles into one frame 
        /// according to the time stamp
        /// </summary>
        private void MergeMotionFrames()
        {
            //List<MotionFrame> ordered = rawMotionSequence.OrderBy(x => x.TimeStamp).ToList();
            //List<MotionFrame> grouped = ordered.GroupBy(u => u.TimeStamp).ToList();
            var v =
                from mf in RawMFSeq
                group mf by mf.TimeStamp into g
                orderby g.Key
                select g;

            foreach (var mfGroup in v)
            {
                Console.WriteLine("TimeStamp: " + mfGroup.Key);
                MotionFrame combinedMf = new MotionFrame();
                foreach (var mf in mfGroup)
                {
                    // This is incorrect!!!
                    combinedMf = MotionFrame.CombineFrame(combinedMf, mf);
                    Console.WriteLine("   " + mf.BehaviorName + "->" + mf.BehaviorName);
                }

                OrderedMFSeq.Add(combinedMf);
            }
        }
Пример #8
0
        /// <summary>
        /// Combine two poses that can coexist simultaneously;
        /// it checks if their TimeStamps are equal!
        /// Some fields take those of this MF!
        /// Joint-ified! JointsToList() is called inside!
        /// </summary>
        /// <param name="mf"></param>
        /// <returns>Return a deep copy of the merged MotionFrame.</returns>
        public static MotionFrame CombineFrame(MotionFrame mf1, MotionFrame mf2)
        {
            // check
            if (mf1.TimeStamp != mf2.TimeStamp)
            {
                Debug.WriteLine("Inconsistent TimeStamp!");
                return null;
            }

            MotionFrame comboMF;
            comboMF = new MotionFrame();
            comboMF.TimeStamp = mf1.TimeStamp;

            // Combined name
            comboMF.BehaviorName = mf1.BehaviorName + "|" + mf2.BehaviorName;
            // required by NaoQi API
            comboMF.IsAbsolute = mf1.IsAbsolute; // ignore mf.isAbsolute;
            // Mixed hold time already presented by TimeStamp!
            comboMF.HoldTime = 0;
            //
            comboMF.UsingSpeedFraction = mf1.UsingSpeedFraction; // ignore mf.UsingSpeedFraction
            comboMF.SpeedFraction = mf1.SpeedFraction; // ignore mf.SpeedFraction

            // Combine the pose list
            // the duplicate pose in "mf" will be ignored
            comboMF.PoseList = new List<PoseProfile>();
            comboMF.PoseList.AddRange(mf1.PoseList);
            foreach (var p in mf2.PoseList)
            {
                bool nonduplicate = true;
                Dictionary<string, PoseProfile> dupPoseList = new Dictionary<string, PoseProfile>();
                foreach (var out_p in mf1.PoseList) // can also use "comboMF.PoseList"
                {
                    if (p.Joints.Name == out_p.Joints.Name)
                    {
                        nonduplicate = false;
                        dupPoseList.Add(out_p.Joints.Name, out_p); // store ref for later use
                    }
                }
                if (nonduplicate) // not duplicated pose
                {
                    comboMF.PoseList.Add(p);
                }
                else
                {
                    if (dupPoseList.ContainsKey("Head")) // duplicated head pose; "BOTH" hands symtric gestures
                    {
                        dupPoseList["Head"].Joints = new Head(false, ((Head)p.Joints).JointInUseValue[0], 0);
                    }
                }
            }
            comboMF.JointsToList();

            #region Old way
            /*
            // Make the List<PoseProfile> to List<value>
            if (this.ComboJointNames == null)
                this.JointsToList();
            if (mf.ComboJointNames == null)
                mf.JointsToList();

            // Remove head from the second to avoid duplicate
            // TODO: do not add at the first place instead of removing afterwards
            if (mf.PoseList.Count > 1)
            {
                if (mf.PoseList[1].Joints.Name == "Head")
                {
                    mf.PoseList.RemoveRange(1, 1);
                    mf.JointsToList();
                }
            }

            comboMF.ComboJointNames = new List<string>();
            comboMF.ComboJointNames.AddRange(this.ComboJointNames);
            comboMF.ComboJointNames.AddRange(mf.ComboJointNames);
            comboMF.ComboJointValueRadian = new List<float>();
            comboMF.ComboJointValueRadian.AddRange(this.ComboJointValueRadian);
            comboMF.ComboJointValueRadian.AddRange(mf.ComboJointValueRadian);
            comboMF.ComboJointSpeedT = new List<float>();
            comboMF.ComboJointSpeedT.AddRange(this.ComboJointSpeedT);
            comboMF.ComboJointSpeedT.AddRange(mf.ComboJointSpeedT);
            */
            #endregion Old way

            return comboMF;
        }
        /// <summary>
        /// Form MotionTimeLine using pose list.
        /// Repetition is handled, and its fast repetition hold-time.
        /// </summary>
        /// <param name="lpp"></param>
        /// <param name="motspd"></param>
        /// <param name="decspd"></param>
        /// <param name="holdtime"></param>
        /// <param name="rep"></param>
        /// <returns></returns>
        protected MotionTimeline CreateMotionTimeline(List<MotionFrame> lmf, double decspd, double drep)
        {
            //!!! Repetition should be done in the pose level, i.e., before Joint-list-ification
            //!!! because in one frame, some poses may be repeated, but others may not.
            // repetition
            int rep = (int)drep;
            List<MotionFrame> mergedframes = lmf;

            if (rep > 0)
            {
                for (int i = 0; i < mergedframes.Count; i++)
                {
                    MotionFrame curmf = mergedframes[i];
                    // It is impossible to only repeat the last frame.
                    // A return pose is always necessary.
                    if (curmf.IsRepeated == true && i < mergedframes.Count-1)
                    {
                        List<MotionFrame> repeatedframes = new List<MotionFrame>();

                        // The 1st repeated motion frame
                        MotionFrame repmf = new MotionFrame(curmf, false);
                        foreach (PoseProfile pp in curmf.PoseList)
                        {
                            if (pp.IsRepeated==true)
                            {
                                repmf.PoseList.Add(pp);
                            }
                        }

                        repeatedframes.Add(repmf);

                        // Search next successive repeated frames
                        int k;
                        for (k = 1; k < mergedframes.Count - i; k++)
                        {
                            curmf = mergedframes[i + k];
                            if (curmf.IsRepeated == false)
                            {
                                break;
                            }
                            else
                            {
                                MotionFrame repmf1 = new MotionFrame(curmf, false);
                                foreach (PoseProfile pp in curmf.PoseList)
                                {
                                    if (pp.IsRepeated == true)
                                    {
                                        repmf1.PoseList.Add(pp);
                                    }
                                }
                                repeatedframes.Add(new MotionFrame(repmf1));
                            }
                        }

                        // Make the last non-repeated frame hold shorter
                        MotionFrame lnrf = mergedframes[i+k-1];
                        Double orighold = lnrf.HoldTime;
                        for (int u = 0; u < lnrf.PoseList.Count; u++)
                        {
                            if (!Double.IsNaN(lnrf.PoseList[u].FastRepeatHoldTime))
                            {
                                lnrf.HoldTime = lnrf.PoseList[u].FastRepeatHoldTime;
                                break;
                            }
                        }

                        // interpolation
                        if (repeatedframes.Count>=2)
                        {
                            for (int n = 0; n < repeatedframes.Count; n++)
                            {
                                MotionFrame cmf = repeatedframes[n];

                                for (int m=0; m<cmf.PoseList.Count; m++)
                                {
                                    // Interpolation
                                    PoseProfile cp = cmf.PoseList[m];
                                    // assume same order
                                    if (cp.RepeatInterpol < 1 && n + 1 < repeatedframes.Count)
                                    {
                                        MotionFrame nmf = repeatedframes[n+1];
                                        PoseProfile np = nmf.PoseList[m];
                                        cmf.PoseList[m] = cp.InterpolationPose(np, cp.RepeatInterpol);
                                    }
                                }
                            }
                        }

                        // Fast repeat
                        foreach (MotionFrame mf in repeatedframes)
                        {
                            foreach (PoseProfile pp in mf.PoseList)
                            {
                                if (!Double.IsNaN(pp.FastRepeatHoldTime))
                                {
                                    mf.HoldTime = pp.FastRepeatHoldTime;
                                    mf.SpeedFraction = 0.35;
                                }
                            }
                        }

                        // repeat these frames
                        List<MotionFrame> allrepeatedframes = new List<MotionFrame>();
                        for (int j = 0; j < rep; j++)
                        {
                            // Deep copy
                            List<MotionFrame> repeatedframes_ = repeatedframes.Select(x => new MotionFrame(x)).ToList();
                            allrepeatedframes.AddRange(repeatedframes_);
                        }

                        // Make the last repeated motion frame hold as long as the last non-repeated motion frame holds originally
                        allrepeatedframes.Last<MotionFrame>().HoldTime = orighold;

                        mergedframes.InsertRange(i + k, allrepeatedframes);

                        // skip to the next original element
                        i += k + rep * repeatedframes.Count;
                    }
                }
            }

            // Joint-list-ify
            foreach (MotionFrame mf_ in mergedframes)
            {
                mf_.JointsToList();
            }

            // Time Line
            MotionTimeline mtl = new MotionTimeline();
            mtl.BehaviorsDesp = this.BehaviorName;

            // finalize frames
            mtl.OrderedMFSeq.AddRange(mergedframes);
            // decay
            mtl.PoseRecover = this.BehaviorRecover;
            mtl.RecovSpd = decspd;

            return mtl;
        }
        /// <summary>
        /// 
        /// </summary>
        /// <param name="lpp"></param>
        /// <param name="motspd"></param>
        /// <param name="holdlist"></param>
        /// <returns></returns>
        protected List<MotionFrame> CreateMotionFrameList(List<PoseProfile> lpp, double motspd, List<double> holdlist)
        {
            // Merge joint-chains of the same ID to one frame.
            // The order of joint-chains in lpp can be random,
            // but the first emergence of a ID determines the resulting order.
            Dictionary<string, MotionFrame> mf_dic = new Dictionary<string, MotionFrame>();
            int curtimestamp = 0;
            int framecnt = 0;
            for (int i = 0; i < lpp.Count; i++)
            {
                PoseProfile pp = lpp[i];
                string mfname = pp.ID;
                if (!mf_dic.ContainsKey(mfname))
                {
                    MotionFrame mf = new MotionFrame();
                    mf.TimeStamp = curtimestamp++;
                    mf.PoseList.Add(pp);
                    mf.PoseID = mfname;
                    mf.SpeedFraction = motspd;
                    mf.HoldTime = holdlist[framecnt++];
                    // false: using "angleInterpolation()";
                    // true: using "angleInterpolationWithSpeed()"
                    mf.UsingSpeedFraction = this.UseSpeedFraction;
                    mf.IsAbsolute = true;

                    mf_dic.Add(mfname, mf);
                }
                else
                {
                    mf_dic[mfname].PoseList.Add(pp);
                }
            }

            return mf_dic.Values.ToList<MotionFrame>();
        }