// 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); } } } }
/// <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); } } }
/// <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; }
/// <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; }
/// <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); } }
/// <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>(); }