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