private void ExecuteMotionTimeLine(MotionTimeline mtl)
{
if (mtl.PoseRecover)
{
//Debug.WriteLine("Initial poses are about to be stored for: " + mtl.BehaviorsDesp);
StorePoses(mtl.BehaviorsDesp, mtl.ContainedJointChainNames);
}
// Hold for "PreDelay" time before execute the behavior
Thread.Sleep(mtl.PreMotionHold);
while (mtl.OrderedMFSeq.Count > 0 && MotionRunning == true)
{
MotionFrame mf = mtl.OrderedMFSeq[0];
mtl.OrderedMFSeq.RemoveAt(0);
// execute
threadAngleInterpolation(mf);
// hold time
int t = (int)Math.Round(1000 * mf.HoldTime);
Thread.Sleep(t);
// for safety
// Thread.Sleep(100);
}
// Hold the last pose for "PostDelay" time
Thread.Sleep(mtl.PostMotionHold);
// Recover initial pose
if (mtl.PoseRecover)
{
//Debug.WriteLine("NaoClient: Pose is about to decay for: " + mtl.BehaviorsDesp);
RecoverPoses(mtl.BehaviorsDesp, (float)mtl.RecovSpd);
}
// Behavior finished
MotionRunning = false;
CurMtl = null;
//Debug.WriteLine("NaoClient: Behavior finished: " + mtl.BehaviorsDesp);
// inform outer context
// Event to inform the behavior is finished
if (evBehaviorFinished != null)
{
evBehaviorFinished(mtl.BehaviorsDesp);
}
}
/// <summary>
/// This is a background worker
/// </summary>
/// <param name="mtl"></param>
public void ExecuteBehaviorInQueue(MotionTimeline mtl)
{
if (proxyMotion != null)
{
/// doesn't work with 1.10.52
//WaitAndKillPreviousBehavior(2000);
CurMtl = mtl;
ThreadPool.QueueUserWorkItem(new WaitCallback(queueMotionDoWork), mtl);
}
else
{
Debug.WriteLine("{0}: {1}", "NaoClient", "Proxy is NULL!");
evBehaviorFinished(mtl.BehaviorsDesp);
}
}
/// <summary>
/// This is a background worker
/// </summary>
/// <param name="mtl"></param>
public void ExecuteBehaviorAsync(MotionTimeline mtl)
{
if (proxyMotion != null)
{
/// doesn't work with 1.10.52
//WaitAndKillPreviousBehavior(2000);
BackgroundWorker bgwMotion = new BackgroundWorker();
bgwMotion.DoWork += new DoWorkEventHandler(bgwMotionDoWork);
bgwMotion.RunWorkerAsync(mtl);
}
else
{
Debug.WriteLine("{0}: {1}", "NaoClient", "Proxy is NULL!");
evBehaviorFinished(mtl.BehaviorsDesp);
}
}
// 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;
}
public static MotionTimeline MergeLeftRight(MotionTimeline mtl_right, MotionTimeline mtl_left)
{
MotionTimeline mtl = new MotionTimeline();
if (mtl_right.BehaviorsDesp == mtl_left.BehaviorsDesp)
mtl.BehaviorsDesp = mtl_right.BehaviorsDesp;
else
mtl.BehaviorsDesp = mtl_right.BehaviorsDesp + "|" + mtl_left.BehaviorsDesp;
mtl.PoseRecover = mtl_right.PoseRecover;
mtl.RecovSpd = mtl_right.RecovSpd;
foreach(MotionFrame mf_r in mtl_right.OrderedMFSeq)
{
// looking for MotionFrame with the same TimeStamp in mtl_left
for (int i = 0; i < mtl_left.OrderedMFSeq.Count; i++)
{
MotionFrame mf_l = mtl_left.OrderedMFSeq[i];
if (mf_r.TimeStamp == mf_l.TimeStamp)
{
MotionFrame mf_new = MotionFrame.CombineFrame(mf_r, mf_l);
mtl.OrderedMFSeq.Add(mf_new);
}
}
}
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>
/// 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;
}
