protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double amplitude = Parameters["Amplitude"].Value;
double headud = Parameters["HeadUpDown"].Value;
// degree
//double headyaw = 30 * amplitude + 15;
double headyaw = base.NormalizeHeadYaw(amplitude);
double headyaw_L = headyaw;
double headyaw_R = -headyaw;
// degree
double headpitch = base.NormalizeHeadPitch(headud);
Head head_L = new Head(false, headpitch, headyaw_L);
Head head_R = new Head(false, headpitch, headyaw_R);
// Joints
PoseProfile pose_left_head = new PoseProfile("LeftHeadPose", head_L, true, 1, 0);
PoseProfile pose_right_head = new PoseProfile("RightHeadPose", head_R, true, 1, 0);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_left_head);
lpp.Add(pose_right_head);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double amplitude = Parameters["Amplitude"].Value;
// degree
double headPitch_b = -20 * amplitude;
double headPitch_f = 11.5 * amplitude + 18;
//double headPitch_b = base.NormalizeHeadPitch(amplitude);
//double headPitch_f = base.NormalizeHeadPitch(-amplitude);
//
double headYaw = 0;
Head head_b = new Head(false, headPitch_b, headYaw);
Head head_f = new Head(false, headPitch_f, headYaw);
// fast hold
double fasthold;
if (amplitude == 0) fasthold = 0.01;
else fasthold = 0;
// Joints
PoseProfile pose_back_head = new PoseProfile("BackPose", head_b, true, 1, fasthold);
PoseProfile pose_front_head = new PoseProfile("FrontPose", head_f, true, 1, fasthold);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_back_head);
lpp.Add(pose_front_head);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double finger = Parameters["FingerRigidness"].Value;
double hand = 0.4 * finger + 0.6;
double headud = Parameters["HeadUpDown"].Value;
double headpitch = 29 * headud * headud - 73.5 * headud + 29.5;
double gaze = Parameters["Gaze"].Value;
double head_right = -(10 * gaze + 10);
double head_left = 10 * gaze + 10;
Head head_r = new Head(false, headpitch, head_right);
Head head_l = new Head(false, headpitch, head_left);
RArm prepose = new RArm(false, 60, -10, 70, 60, 0, hand);
PoseProfile prepose_arm = new PoseProfile("PrePose", prepose);
List<PoseProfile> lpp1 = RightArmPointRightSonar("RightSonar");
PoseProfile gaze_right = new PoseProfile("RightSonar", head_r);
List<PoseProfile> lpp2 = RightArmPointLeftSonar("LeftSonar");
PoseProfile gaze_left = new PoseProfile("LeftSonar", head_l);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(prepose_arm);
lpp.AddRange(lpp1);
lpp.Add(gaze_right);
lpp.AddRange(lpp2);
lpp.Add(gaze_left);
return lpp;
}
/// <summary>
/// TODO: probabilistic model to select the movements that are less selected
/// </summary>
/// <param name="right"></param>
/// <returns></returns>
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double LeftLook = RandomGen.Next(MinHeadAmp, MaxHeadAmp) / 10.0D;
double RightLook = -RandomGen.Next(MinHeadAmp, MaxHeadAmp) / 10.0D;
System.Diagnostics.Debug.WriteLine("HeadRandomScan: Left {0} right {1}", LeftLook, RightLook);
double headud = Parameters["HeadUpDown"].Value;
double headpitch = base.NormalizeHeadPitch(headud);
Head head_left = new Head(false, headpitch, LeftLook);
Head head_right = new Head(false, headpitch, RightLook);
// Joints
PoseProfile pose_head_left = new PoseProfile("PoseL", head_left);
PoseProfile pose_head_right = new PoseProfile("PoseR", head_right);
List<PoseProfile> lpp = new List<PoseProfile>();
// sometimes first look left; sometimes right
int lr = RandomGen.Next(0,1);
if (lr == 0)
{
lpp.Add(pose_head_left);
lpp.Add(pose_head_right);
}
else
{
lpp.Add(pose_head_right);
lpp.Add(pose_head_left);
}
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double width = Parameters["Width"].Value;
double height = Parameters["Height"].Value;
double amplitude = Parameters["Amplitude"].Value;
double palmdir = Parameters["PalmDir"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
// width
double shldroll = -10 * width;
double elbyaw = -10 * width + 80;
double shldptch = -20 * height + 80;
double elbroll = -10 * height + 70;
double wristyaw = 10 * palmdir;
double hand = 0.3 * finger + 0.5;
double delta_elbroll = 8 * amplitude + 10;
double delta_shldptch = 8 * amplitude + 8;
// high pose
double shldptch_high = shldptch - delta_shldptch;
double elbroll_high = elbroll + delta_elbroll;
// low pose
double shldptch_low = shldptch + delta_shldptch;
double elbroll_low = elbroll - delta_elbroll;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// degree
double headYaw = 0;
Head head = new Head(false, headPitch, headYaw);
RArm rarm_high = new RArm(false, shldptch_high, shldroll, elbyaw, elbroll_high, wristyaw, hand);
RArm rarm_low = new RArm(false, shldptch_low, shldroll, elbyaw, elbroll_low, wristyaw, hand);
LArm larm_high = (LArm)rarm_high.MirrorLeftRight();
LArm larm_low = (LArm)rarm_low.MirrorLeftRight();
// Joints
PoseProfile pose_head = new PoseProfile("Pose1", head);
PoseProfile pose_larm_high = new PoseProfile("Pose1", larm_high);
PoseProfile pose_larm_low = new PoseProfile("Pose2", larm_low, true);
PoseProfile pose_rarm_high = new PoseProfile("Pose2", rarm_high, true);
PoseProfile pose_rarm_low = new PoseProfile("Pose3", rarm_low, true);
PoseProfile pose_larm_high1 = new PoseProfile("Pose3", larm_high, true);
PoseProfile pose_larm_low1 = new PoseProfile("Pose4", larm_low);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_head);
lpp.Add(pose_larm_high);
lpp.Add(pose_larm_low);
lpp.Add(pose_rarm_high);
lpp.Add(pose_rarm_low);
lpp.Add(pose_larm_high1);
lpp.Add(pose_larm_low1);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double width = Parameters["Width"].Value;
double height = Parameters["Height"].Value;
double palmdir = Parameters["PalmDir"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
double shoulderPitch_open = -20*height+90;
double shoulderRoll_open = -20 * width -10;
double elbowYaw_open = 29.5*width + 90;
double elbowRoll_open = 88.5;
double wristYaw_open = 60*palmdir;
double hand_open = 1;
double shoulderPitch_close = -25*height+70;
double shoulderRoll_close = -10*width+10;
double elbowYaw_close = -10*width+65;
double elbowRoll_close = 88.5;
double wristYaw_close = -20;
double hand_close = 0.2 * finger + 0.6;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// degree
double headYaw = double.NaN;
RArm rarmposeopen = new RArm(false,
shoulderPitch_open, shoulderRoll_open,
elbowYaw_open, elbowRoll_open,
wristYaw_open, hand_open);
LArm larmposeopen = (LArm)rarmposeopen.MirrorLeftRight();
RArm rarmposeclose = new RArm(false,
shoulderPitch_close, shoulderRoll_close,
elbowYaw_close, elbowRoll_close,
wristYaw_close, hand_close);
LArm larmposeclose = (LArm)rarmposeclose.MirrorLeftRight();
Head head = new Head(false, headPitch, headYaw);
PoseProfile arm_r_pose_open = new PoseProfile("OpenPose", rarmposeopen, true, 0.3, 0);
PoseProfile arm_l_pose_open = new PoseProfile("OpenPose", larmposeopen, true, 0.3, 0);
PoseProfile head_pose = new PoseProfile("OpenPose", head);
PoseProfile arm_r_pose_close = new PoseProfile("ClosePose", rarmposeclose, true, 1, 0);
PoseProfile arm_l_pose_close = new PoseProfile("ClosePose", larmposeclose, true, 1, 0);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(arm_r_pose_open);
lpp.Add(arm_l_pose_open);
lpp.Add(head_pose);
lpp.Add(arm_r_pose_close);
lpp.Add(arm_l_pose_close);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double amp = Parameters["Amplitude"].Value;
double height = Parameters["Height"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double lsr = 3*height-18;
double rsr = 15*height;
double lsp = 10*height+50;
double rsp = 30*height+30;
double ley = 5*height-5;
double rey = 25*height+15;
double ler = 15*height-50;
double rer = -20*height+80;
double lwy = -90;
double rwy = -45;
// finger 0~1
double hand = 0.6 * finger;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// -30 ~ 30 degree
double headYaw_L = 10.0 * amp + 20;
double headYaw_R = -10.0 * amp - 20;
LArm larm = new LArm(false, lsp, lsr, ley, ler, lwy, hand);
RArm rarm = new RArm(false, rsp, rsr, rey, rer, rwy, hand);
Head lhead = new Head(false, headPitch, headYaw_L);
Head rhead = new Head(false, headPitch, headYaw_R);
// Joints
PoseProfile pose_close_arm_l = new PoseProfile("ArmClose", larm);
PoseProfile pose_close_arm_r = new PoseProfile("ArmClose", rarm);
PoseProfile pose_look_left;
PoseProfile pose_look_right;
if (headud >= 0) // positive
{
pose_look_left = new PoseProfile("LookLeft", lhead, true, 0, 0.05);
pose_look_right = new PoseProfile("LookRight", rhead, true, 0, 0.05);
}
else // negative
{
pose_look_left = new PoseProfile("LookLeft", lhead, true);
pose_look_right = new PoseProfile("LookRight", rhead, true);
}
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_close_arm_l);
lpp.Add(pose_close_arm_r);
lpp.Add(pose_look_left);
lpp.Add(pose_look_right);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double fpwidth = Parameters["FrontPoseWidth"].Value;
double fpheight = Parameters["FrontPoseHeight"].Value;
double cpheight = Parameters["ChestPoseHeight"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
double fp_shldroll = -30*fpwidth;
double fp_shldptch = -30*fpheight+60;
double fp_elbyaw = 89.5*fpwidth+30;
double fp_elbroll = 30*fpheight+30;
double fp_wrstyaw = 30;
double cp_shldroll = 5;
double cp_shldptch = -30*cpheight+60;
double cp_elbyaw = -20*cpheight+30;
double cp_elbroll = 88.5;
double cp_wrstyaw = 30;
// finger 0~1
double hand = 0.4 * finger + 0.6;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// - ~ degree
double headYaw;
if (headlr > 0.5) headYaw = -40 * headlr + 20;
else headYaw = 0;
RArm rarm_front = new RArm(false, fp_shldptch, fp_shldroll, fp_elbyaw, fp_elbroll, fp_wrstyaw, hand);
Head head = new Head(false, headPitch, headYaw);
RArm rarm_chest = new RArm(false, cp_shldptch, cp_shldroll, cp_elbyaw, cp_elbroll, cp_wrstyaw, hand);
LArm larm = new LArm(false, 84.6, 15, -68, -40, 5.7, 0.3);
// Joints
PoseProfile pose_front_arm_r = new PoseProfile("FrontPose", rarm_front, true);
PoseProfile pose_front_head = new PoseProfile("FrontPose", head);
PoseProfile pose_front_arm_l = new PoseProfile("FrontPose", larm);
PoseProfile pose_chest_arm_r = new PoseProfile("ChestPose", rarm_chest, true);
List<PoseProfile> lppbow = BendTorsoSmall("FrontPose");
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_front_arm_r);
lpp.Add(pose_front_head);
lpp.Add(pose_front_arm_l);
lpp.Add(pose_chest_arm_r);
lpp.AddRange(lppbow);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double width = Parameters["Width"].Value;
double height = Parameters["Height"].Value;
double palmdir = Parameters["PalmDirection"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
// TODO move this initial pose to base class
double rshoulderroll_init = 0;
double rshoulderpitch_init = 45;
double relbowroll_init = 88.5;
double relbowyaw_init = 70;
double rwristyaw_init = 60;
double rhand_init = 0.6;
double rshoulderroll = -30 * width;
double rshoulderpitch = -50 * height + 60;
double relbowroll = -43 * height + 45;
double relbowyaw = 90;
double rwristyaw = 90 * palmdir;
double rhand = 0.4 * finger + 0.6;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// degree
double headYaw;
if (headlr > 0.5)
{
headYaw = -40 * headlr + 20;
}
else
{
headYaw = double.NaN;
}
RArm rarm_init = new RArm(false, rshoulderpitch_init, rshoulderroll_init, relbowyaw_init, relbowroll_init, rwristyaw_init, rhand_init);
Head head = new Head(false, headPitch, headYaw);
RArm rarm_end = new RArm(false, rshoulderpitch, rshoulderroll, relbowyaw, relbowroll, rwristyaw, rhand);
// Joints
PoseProfile pose_rarm_init = new PoseProfile("InitPose", rarm_init);
PoseProfile pose_head_init = new PoseProfile("InitPose", head);
PoseProfile pose_rarm_end = new PoseProfile("EndPose", rarm_end);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_rarm_init);
lpp.Add(pose_head_init);
lpp.Add(pose_rarm_end);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double amp = Parameters["Amplitude"].Value;
double palmdir = Parameters["PalmDirection"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
double rshoulderroll_high = -10*amp-20;
double rshoulderpitch_high = -20*amp+40;
double relbowroll_high = 5*amp+40;
double relbowyaw_high = 119.5;
double rwristyaw_high = 30 * palmdir - 90;
double rhand_high = 0.6;
double rshoulderroll_low = -5*amp-5;
double rshoulderpitch_low = -10 * amp + 90;
double relbowroll_low = -5 * amp + 60;
double relbowyaw_low = 119.5;
double rwristyaw_low = 30 * palmdir - 90;
double rhand_low = 0.3 * finger + 0.6;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// degree
double headYaw;
if (headlr > 0.5)
{
headYaw = -40 * headlr + 20;
}
else
{
headYaw = double.NaN;
}
RArm rarm_high = new RArm(false, rshoulderpitch_high, rshoulderroll_high,
relbowyaw_high, relbowroll_high, rwristyaw_high, rhand_high);
Head head = new Head(false, headPitch, headYaw);
RArm rarm_low = new RArm(false, rshoulderpitch_low, rshoulderroll_low,
relbowyaw_low, relbowroll_low, rwristyaw_low, rhand_low);
// Joints
PoseProfile pose_rarm_init = new PoseProfile("HighPose", rarm_high, true, 0, 0.03);
PoseProfile pose_head_init = new PoseProfile("HighPose", head);
PoseProfile pose_rarm_low = new PoseProfile("LowPose", rarm_low, true, 1.0, 0.03);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_rarm_init);
lpp.Add(pose_head_init);
lpp.Add(pose_rarm_low);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double bodyleanforward = Parameters["BodyLeanForward"].Value;
double width = Parameters["Width"].Value;
double height = Parameters["Height"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
//
double wristyaw = 30;
// Down pose
double up_shldroll = -20 * width - 10;
double up_elbyaw = 30*width+60;
double up_shldpitch = -30 * height - 30;
double up_elbroll = -58*height+60;
double up_wristyaw = wristyaw;
double up_hand = 0.2*finger+0.8;
// Down pose
double dp_shldroll = -15 * width;
double dp_elbyaw = 20*width+45;
double dp_shldpitch = 45 * height + 45;
double dp_elbroll = 18.5 * height + 70;
double dp_wristyaw = wristyaw;
double dp_hand = -0.4*finger+0.4;
// degree
double headpitch = base.NormalizeHeadPitch(headud);
// degree
double headYaw = 0;
Head head = new Head(false, headpitch, headYaw);
RArm rarm_up = new RArm(false, up_shldpitch, up_shldroll, up_elbyaw, up_elbroll, up_wristyaw, up_hand);
RArm rarm_down = new RArm(false, dp_shldpitch, dp_shldroll, dp_elbyaw, dp_elbroll, dp_wristyaw, dp_hand);
// Joints
PoseProfile pose_head = new PoseProfile("UpPose", head);
PoseProfile pose_rarm_up = new PoseProfile("UpPose", rarm_up);
PoseProfile pose_rarm_down = new PoseProfile("DownPose", rarm_down);
List<PoseProfile> legpp = base.BodyLeanForward("DownPose", bodyleanforward);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_head);
lpp.Add(pose_rarm_up);
lpp.Add(pose_rarm_down);
lpp.AddRange(legpp);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double amp = Parameters["Amplitude"].Value;
double height = Parameters["Height"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double gazehand = Parameters["GazeHand"].Value;
//
double wristyaw = 90;
// Down pose
double up_shldroll = -15 * amp - 45;
double up_elbyaw = 0;
double up_shldpitch = 15 * height - 45;
double up_elbroll = -43 * height + 45;
double up_wristyaw = wristyaw;
double up_hand = 0.2 * finger + 0.8;
double up_headYaw = -30 * gazehand + 30;
// Down pose
double dp_shldroll = -15 * amp - 45;
double dp_elbyaw = 0;
double dp_shldpitch = -30 * height - 60;
double dp_elbroll = 88.5;
double dp_wristyaw = wristyaw;
double dp_hand = -0.4 * finger + 0.4;
double dp_headYaw = -30*gazehand-60;
// degree
double headpitch = base.NormalizeHeadPitch(headud);
Head head_up = new Head(false, headpitch, up_headYaw);
Head head_down = new Head(false, headpitch, dp_headYaw);
RArm rarm_up = new RArm(false, up_shldpitch, up_shldroll, up_elbyaw, up_elbroll, up_wristyaw, up_hand);
RArm rarm_down = new RArm(false, dp_shldpitch, dp_shldroll, dp_elbyaw, dp_elbroll, dp_wristyaw, dp_hand);
// Joints
PoseProfile pose_head_up = new PoseProfile("UpPose", head_up);
PoseProfile pose_rarm_up = new PoseProfile("UpPose", rarm_up);
PoseProfile pose_head_down = new PoseProfile("DownPose", head_down);
PoseProfile pose_rarm_down = new PoseProfile("DownPose", rarm_down);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_head_up);
lpp.Add(pose_rarm_up);
lpp.Add(pose_head_down);
lpp.Add(pose_rarm_down);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
// TODO: Empirical data, need to be tested
double headpitch = 12;
Head head = new Head(false, headpitch, 0);
PoseProfile pose_head = new PoseProfile("Pose", head);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_head);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double rpwidth = Parameters["RaisePoseWidth"].Value;
double rpheight = Parameters["RaisePoseHeight"].Value;
double cpheight = Parameters["ChestPoseHeight"].Value;
double palmdir = Parameters["PalmDirection"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
// finger 0~1
double hand = 0.4 * finger + 0.6;
// Raise pose center width
double rp_shldroll = -30 * rpwidth;
double rp_elbyaw = 90;
double rp_shldpitch = -30 * rpheight + 30;
double rp_elbroll = 88.5;
double rp_wristyaw = -110 * palmdir + 90;
double cp_shldroll = 5;
double cp_shldptch = -30 * cpheight + 60;
double cp_elbyaw = -20 * cpheight + 30;
double cp_elbroll = 88.5;
double cp_wrstyaw = 64.5 * palmdir + 40;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// - ~ degree
double headYaw = 0;
if (headlr > 0.5) headYaw = -40 * headlr + 20;
else headYaw = 0;
RArm rarm_front = new RArm(false, rp_shldpitch, rp_shldroll, rp_elbyaw, rp_elbroll, rp_wristyaw, hand);
Head head = new Head(false, headPitch, headYaw);
RArm rarm_chest = new RArm(false, cp_shldptch, cp_shldroll, cp_elbyaw, cp_elbroll, cp_wrstyaw, hand);
// Joints
PoseProfile pose_front_arm_r = new PoseProfile("RaisePose", rarm_front);
PoseProfile pose_front_head = new PoseProfile("RaisePose", head);
PoseProfile pose_chest_arm_r = new PoseProfile("ChestPose", rarm_chest);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_front_arm_r);
lpp.Add(pose_front_head);
lpp.Add(pose_chest_arm_r);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double width = Parameters["Width"].Value;
double height = Parameters["Height"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
double lsr = 30 * width;
double rsr = -30 * width;
double lsp = 10 * height + 60;
double rsp = -55 * height + 20;
double ley = -10 * height - 50;
double rey = 20 * height + 60;
double ler = -45;
double rer = -15 * height + 75;
double lwy = 10 * height - 100;
double rwy = 30 * height - 30;
// finger 0~1
double hand = 0.4 * finger + 0.6;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// degree
double headYaw = double.NaN;
LArm larm = new LArm(false, lsp, lsr, ley, ler, lwy, hand);
RArm rarm = new RArm(false, rsp, rsr, rey, rer, rwy, hand);
Head head = new Head(false, headPitch, headYaw);
LArm larm_close = new LArm(false, 60, -15, -20, -55, -104.5, hand);
RArm rarm_close = new RArm(false, 35, 15, 20, 75, -40, hand);
// Joints
PoseProfile pose_open_arm_l = new PoseProfile("ArmOpen", larm, true, 0.3, 0);
PoseProfile pose_open_arm_r = new PoseProfile("ArmOpen", rarm, true, 0.3, 0);
PoseProfile pose_open_head = new PoseProfile("ArmOpen", head);
PoseProfile pose_close_arm_l = new PoseProfile("ArmClose", larm_close, true, 1, 0);
PoseProfile pose_close_arm_r = new PoseProfile("ArmClose", rarm_close, true, 1, 0);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_open_arm_l);
lpp.Add(pose_open_arm_r);
lpp.Add(pose_open_head);
lpp.Add(pose_close_arm_l);
lpp.Add(pose_close_arm_r);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double width = Parameters["Width"].Value;
double amp = Parameters["Amplitude"].Value;
double palmdir = Parameters["PalmDir"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
double shoulderRoll = -30 * width;
double elbowYaw = 39.5 * width + 80;
double elbowRollLow = -28 * amp + 30;
double elbowRollHigh = 28.5 * amp + 60;
double shoulderPitchLow = 20 * amp + 70;
double shoulderPitchHigh = -10 * amp + 30;
double wristYaw = 74 * palmdir + 30;
// finger 0~1
double hand = 0.4 * finger + 0.6;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// degree
double headYaw = double.NaN;
RArm rarmposelow = new RArm(false,shoulderPitchLow,shoulderRoll,elbowYaw,elbowRollLow,wristYaw,hand);
LArm larmposelow = (LArm)rarmposelow.MirrorLeftRight();
RArm rarmposehigh = new RArm(false,shoulderPitchHigh,shoulderRoll,elbowYaw,elbowRollHigh,wristYaw,hand);
LArm larmposehigh = (LArm)rarmposehigh.MirrorLeftRight();
Head head = new Head(false, headPitch, headYaw);
PoseProfile arm_r_pose_low = new PoseProfile("LowPose", rarmposelow, true, 0.3, 0);
PoseProfile arm_l_pose_low = new PoseProfile("LowPose", larmposelow, true, 0.3, 0);
PoseProfile head_pose = new PoseProfile("LowPose", head);
PoseProfile arm_r_pose_high = new PoseProfile("HighPose", rarmposehigh, true, 1, 0);
PoseProfile arm_l_pose_high = new PoseProfile("HighPose", larmposehigh, true, 1, 0);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(arm_r_pose_low);
lpp.Add(arm_l_pose_low);
lpp.Add(head_pose);
lpp.Add(arm_r_pose_high);
lpp.Add(arm_l_pose_high);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double headud = Parameters["HeadUpDown"].Value;
double headpitch = base.NormalizeHeadPitch(headud);
Head head = new Head(false, headpitch, 0);
// Joints
PoseProfile pose_head = new PoseProfile("Pose", head);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_head);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double width = Parameters["Width"].Value;
double height = Parameters["Height"].Value;
double palmdir = Parameters["PalmDirection"].Value;
double ampfinger = Parameters["AmplitudeFinger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
double rshoulderroll = -10 * width;
double rshoulderpitch = -20 * height + 50;
double relbowroll = 88.5;
double relbowyaw = 30*width+70;
double rwristyaw = -65 * palmdir;
// open
double rhand_open = 0.2*ampfinger + 0.8;
double rhand_close = 0.4;
// ? degree
double headPitch = base.NormalizeHeadPitch(headud);
// -20 ~ 0 degree
double headYaw;
if (headlr > 0.5)
{
headYaw = -40 * headlr + 20;
}
else
{
headYaw = double.NaN;
}
RArm rarm_open = new RArm(false, rshoulderpitch, rshoulderroll, relbowyaw, relbowroll, rwristyaw, rhand_open);
Head head = new Head(false, headPitch, headYaw);
RArm rarm_close = new RArm(false, rshoulderpitch, rshoulderroll, relbowyaw, relbowroll, rwristyaw, rhand_close);
// Joints
PoseProfile pose_rarm_open = new PoseProfile("OpenPose", rarm_open, true, 1.0, 0);
PoseProfile pose_head = new PoseProfile("OpenPose", head);
PoseProfile pose_rarm_close = new PoseProfile("ClosePose", rarm_close, true, 1.0, 0);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_rarm_open);
lpp.Add(pose_head);
lpp.Add(pose_rarm_close);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double fpwidth = Parameters["FrontPoseWidth"].Value;
double fpheight = Parameters["FrontPoseHeight"].Value;
double cpheight = Parameters["ChestPoseHeight"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
double fp_shldroll = 28 * fpwidth - 10;
double fp_shldptch = -50 * fpheight + 60;
double fp_elbyaw = 5 * fpwidth + 85;
double fp_elbroll = -35 * fpheight + 50;
double fp_wrstyaw = 60 * fpheight + 30;
double cp_shldroll = -5 * cpheight + 5;
double cp_shldptch = -40 * cpheight + 60;
double cp_elbyaw = -15 * cpheight + 15;
double cp_elbroll = 15 * cpheight + 70;
double cp_wrstyaw = 59.5 * cpheight + 45;
// finger 0~1
double hand = 0.4 * finger + 0.6;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// ~ degree
double headYaw;
if (headlr > 0.5) headYaw = -20 * headlr + 10;
else headYaw = 0;
RArm rarm_front = new RArm(false, fp_shldptch, fp_shldroll, fp_elbyaw, fp_elbroll, fp_wrstyaw, hand);
Head head = new Head(false, headPitch, headYaw);
RArm rarm_chest = new RArm(false, cp_shldptch, cp_shldroll, cp_elbyaw, cp_elbroll, cp_wrstyaw, hand);
// Joints
PoseProfile pose_front_arm_r = new PoseProfile("FrontPose", rarm_front);
PoseProfile pose_front_head = new PoseProfile("FrontPose", head);
PoseProfile pose_chest_arm_r = new PoseProfile("ChestPose", rarm_chest);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_front_arm_r);
lpp.Add(pose_front_head);
lpp.Add(pose_chest_arm_r);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double width = Parameters["Width"].Value;
double height = Parameters["Height"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headamp = Parameters["HeadAmplitude"].Value;
double shldroll = -28 * width+18;
double shldptch = -95 * height + 35;
double elbyaw = 35;
double elbroll = -23.5 * height + 88.5;
double wrstyaw = 45 * height + 30;
// finger 0~1
double hand = 0.8 * finger;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// - ~ degree
double headYaw_L = 18 * headamp + 18;
double headYaw_R = -14 * headamp - 14;
RArm rarm = new RArm(false, shldptch, shldroll, elbyaw, elbroll, wrstyaw, hand);
Head head_l = new Head(false, headPitch, headYaw_L);
Head head_r = new Head(false, headPitch, headYaw_R);
// Joints
PoseProfile pose_arm = new PoseProfile("ArmPose", rarm);
PoseProfile pose_head_l = new PoseProfile("HeadPoseLeft", head_l, true, 1, 0);
PoseProfile pose_head_r = new PoseProfile("HeadPoseRight", head_r, true, 1, 0);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_arm);
lpp.Add(pose_head_l);
lpp.Add(pose_head_r);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double width = Parameters["Width"].Value;
double height = Parameters["Height"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
// right
double shldroll_r =-15*width -5;
double elbyaw_r =90;
double shldptch_r =-40*height+90;
double elbroll_r =28.5*height+60;
double wristyaw_r =74.5*height+30;
double hand_r =0.3*finger+0.5;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
double headYaw = double.NaN;
Head head = new Head(false, headPitch, headYaw);
RArm rarm_one = new RArm(false, shldptch_r, shldroll_r,
elbyaw_r, elbroll_r, wristyaw_r, hand_r);
LArm larm_theother = (LArm)rarm_one.MirrorLeftRight();
// Joints
PoseProfile pose_head = new PoseProfile("Prepare", head);
List<PoseProfile> lpp_prepare = base.ArmChest("Prepare");
PoseProfile pose_rarm_one = new PoseProfile("One", rarm_one);
PoseProfile pose_larm_theother = new PoseProfile("TheOther", larm_theother);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_head);
lpp.AddRange(lpp_prepare);
lpp.Add(pose_rarm_one);
lpp.Add(pose_larm_theother);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double headud = Parameters["HeadUpDown"].Value;
double headpitch = -15 * headud;
Head head = new Head(false, headpitch, 0);
// front
List<PoseProfile> lpp1 = RightArmPointFrontMic("FrontMic");
PoseProfile head_pose = new PoseProfile("FrontMic", head);
// side
List<PoseProfile> lpp2 = RightArmPointSideMic("SideMic");
// rear
List<PoseProfile> lpp3 = RightArmPointRearMic("RearMic");
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.AddRange(lpp1);
lpp.Add(head_pose);
lpp.AddRange(lpp2);
lpp.AddRange(lpp3);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double width = Parameters["Width"].Value;
double height = Parameters["Height"].Value;
double palmdir = Parameters["PalmDir"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
// finger 0~1
double hand = 0.4 * finger + 0.6;
double wristyaw = 60 * palmdir + 30;
double elbyaw = 90;
double shldroll = -20 * width - 20;
double shldpitch = -40 * height + 60;
double elbroll = -28 * height + 30;
// degree
double headpitch = base.NormalizeHeadPitch(headud);
// degree
double headYaw = double.NaN;
Head head = new Head(false, headpitch, headYaw);
RArm rarm = new RArm(false, shldpitch, shldroll, elbyaw, elbroll, wristyaw, hand);
// Joints
PoseProfile pose_head = new PoseProfile("Pose", head);
PoseProfile pose_rarm = new PoseProfile("Pose", rarm);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_head);
lpp.Add(pose_rarm);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double width = Parameters["Width"].Value;
double forward = Parameters["Forward"].Value;
double palmdir = Parameters["PalmDir"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
// up inward pose
double up_in_shldpitch = -10 * width + 40;
double up_in_shldroll = 6 * width + 12;
double up_in_elbyaw = -10 * width + 70;
// up outward pose
double up_out_shldpitch = -10 * width + 45;
double up_out_shldroll = -3 * width - 12;
double up_out_elbyaw = 10 * width + 100;
// down inward pose
double down_in_shldpitch = 5 * width + 60;
double down_in_shldroll = width + 8;
double down_in_elbyaw = 5 * width + 75;
// down outward pose
double down_out_shldpitch = 10 * width + 65;
double down_out_shldroll = -2 * width - 8;
double down_out_elbyaw = 5 * width + 95;
// forward
double elbroll = -43.5 * forward + 88.5;
// palm dir
double wristyaw = 90 * palmdir;
// finger
double hand = 0.4 * finger + 0.6;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// degree
double headYaw;
if (headlr > 0.5)
{
headYaw = -36 * headlr + 18;
}
else
{
headYaw = double.NaN;
}
Head head = new Head(false, headPitch, headYaw);
RArm rarm_upinward = new RArm(false, up_in_shldpitch, up_in_shldroll, up_in_elbyaw, elbroll, wristyaw, finger);
RArm rarm_upoutward = new RArm(false, up_out_shldpitch, up_out_shldroll, up_out_elbyaw, elbroll, wristyaw, finger);
RArm rarm_downinward = new RArm(false, down_in_shldpitch, down_in_shldroll, down_in_elbyaw, elbroll, wristyaw, finger);
RArm rarm_downoutward = new RArm(false, down_out_shldpitch, down_out_shldroll, down_out_elbyaw, elbroll, wristyaw, finger);
// Joints
PoseProfile pose_head = new PoseProfile("upin", head);
PoseProfile pose_rarm_upin = new PoseProfile("upin", rarm_upinward);
PoseProfile pose_rarm_upout = new PoseProfile("upout", rarm_upoutward, true);
PoseProfile pose_rarm_downin = new PoseProfile("downin", rarm_downinward, true);
PoseProfile pose_rarm_downout = new PoseProfile("downout", rarm_downoutward, true);
PoseProfile pose_rarm_upin_back = new PoseProfile("upinback", rarm_upinward, true);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_head);
lpp.Add(pose_rarm_upin);
lpp.Add(pose_rarm_upout);
lpp.Add(pose_rarm_downout);
lpp.Add(pose_rarm_downin);
lpp.Add(pose_rarm_upin_back);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double openness = Parameters["PalmUpDown"].Value;
double fingerrigidness = Parameters["Finger"].Value;
double amplitude = Parameters["Amplitude"].Value;
double headupdown = Parameters["HeadUpDown"].Value;
double headleftright = Parameters["HeadLeftRight"].Value;
double hand = 0.01 * fingerrigidness + 0.9; // *180D / Math.PI;
double amp = amplitude * 218.666;
double elbowRoll = Math.Acos((amp * amp - Math.Pow(105D, 2D) - Math.Pow(57.75 + 55.95, 2D)) / (2D * 105D * (55.95 + 57.75)));
elbowRoll *= 180D / Math.PI;
double wristYaw;
if (this.Direction == "Up")
{
wristYaw = -70;
//-(10D * openness * openness + 80D * openness + 10D);
}
else
{
wristYaw = 10D * openness * openness + 80D * openness + 10D;
}
// Determine Shoulder Pitch, Roll
// Set to 80 to make the "wristYaw" able to reach "PalmUp" and "PalmDown"
//double elbowYaw = 80D;
//Microsoft.FSharp.Math.Vector<double> result
// = NaoJointChain.Arm.OptPntTo2JSeed(
// elbowRoll * Math.PI / 180D,
// elbowYaw * Math.PI / 180D,
// TargetX, TargetY, TargetZ,
// SeedShldPtch, SeedShldRoll);
Microsoft.FSharp.Math.Vector<double> result
= NaoJointChain.Arm.OptPntTo3JSeed(
elbowRoll * Math.PI / 180D,
TargetX, TargetY, TargetZ,
SeedShldPtch, SeedShldRoll, SeedElbYaw);
double shoulderPitch = result[0] * 180D / Math.PI;
double shoulderRoll = result[1] * 180D / Math.PI;
double elbowYaw = result[2] * 180D / Math.PI;
//////////////////////////////////////////////////////////////////////////
double headpitch, headyaw;
/*
if (pp.Valence >= 0)
{
Microsoft.FSharp.Math.Vector<double> result1
= NaoJointChain.Head.LookAt(targetX, targetY, targetZ);
headpitch = result1[0] * 180 / Math.PI;
headyaw = result1[1] * 180 / Math.PI;
}
else
{
Microsoft.FSharp.Math.Vector<double> result1
= NaoJointChain.Head.DistractFrom(targetX, targetY, targetZ);
headpitch = result1[0] * 180 / Math.PI;
headyaw = result1[1] * 180 / Math.PI;
}*/
// degree
headpitch = base.NormalizeHeadPitch(headupdown);
// degree
if (this.Direction.Contains("Right"))
{
headyaw = -base.NormalizeHeadYaw(headleftright);
}
else headyaw = 0;
//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
RArm initArm = new RArm(false,
shoulderPitch_1,
shoulderRoll_1,
elbowYaw_1,
elbowRoll_1,
wristYaw_1,
hand_1);
Head head = new Head(false, headpitch, headyaw);
RArm endArm = new RArm(false,
shoulderPitch,
shoulderRoll,
elbowYaw,
elbowRoll,
wristYaw,
hand);
PoseProfile initArmPose = new PoseProfile("InitPose", initArm, true, 0.5);
PoseProfile initHeadPose = new PoseProfile("InitPose", head);
PoseProfile endArmPose = new PoseProfile("EndPose", endArm, true, 1.0, 0.5);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(initArmPose);
lpp.Add(initHeadPose);
lpp.Add(endArmPose);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double palmdir = Parameters["PalmDir"].Value;
double wristyaw;
if (palmdir > 0) wristyaw = 90;
else wristyaw = -90;
double headud = Parameters["HeadUpDown"].Value;
double headpitch = 29 * headud * headud - 73.5 * headud + 29.5;
double gaze = Parameters["Gaze"].Value;
double head_right = -(10 * gaze + 10);
Head head_r = new Head(false, headpitch, head_right);
RArm arm_show_orig = new RArm(false, 0, 0, 90, 2, wristyaw, 0);
PoseProfile pose_show_orig = new PoseProfile("PS_Orig", arm_show_orig);
PoseProfile gaze_right = new PoseProfile("PS_Orig", head_r);
RArm arm_show_shoulderpitch = new RArm(false, 30, 0, 90, 2, wristyaw, 0);
PoseProfile pose_show_shoulderpitch = new PoseProfile("PS_SP", arm_show_shoulderpitch);
PoseProfile pose_show_orig1 = new PoseProfile("PS_Orig1", arm_show_orig);
RArm arm_show_shoulderroll = new RArm(false, 0, -30, 90, 2, wristyaw, 0);
PoseProfile pose_show_shouldroll = new PoseProfile("PS_SR", arm_show_shoulderroll);
PoseProfile pose_show_orig2 = new PoseProfile("PS_Orig2", arm_show_orig);
RArm arm_show_elbowroll = new RArm(false, 0, 0, 90, 88.5, -wristyaw, 0);
PoseProfile pose_show_elbowroll = new PoseProfile("PS_ER", arm_show_elbowroll);
RArm arm_show_elbowyaw1 = new RArm(false, 0, 0, 119.5, 88.5, -wristyaw, 0);
PoseProfile pose_show_elbowyaw1 = new PoseProfile("PS_EY1", arm_show_elbowyaw1);
RArm arm_show_elbowyaw2 = new RArm(false, 0, 0, 60, 88.5, -wristyaw, 0);
PoseProfile pose_show_elbowyaw2 = new PoseProfile("PS_EY2", arm_show_elbowyaw2);
RArm arm_show_wristyaw1 = new RArm(false, 0, 0, 90, 88.5, -wristyaw, 0);
PoseProfile pose_show_wristyaw1 = new PoseProfile("PS_WY1", arm_show_wristyaw1);
RArm arm_show_wristyaw2 = new RArm(false, 0, 0, 90, 88.5, wristyaw, 0);
PoseProfile pose_show_wristyaw2 = new PoseProfile("PS_WY2", arm_show_wristyaw2);
PoseProfile pose_show_orig3 = new PoseProfile("PS_Orig3", arm_show_orig);
RArm arm_show_finger = new RArm(false, 0, 0, 90, 2, wristyaw, 1);
PoseProfile pose_show_finger = new PoseProfile("PS_FG", arm_show_finger);
PoseProfile pose_show_orig4 = new PoseProfile("PS_Orig4", arm_show_orig);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_show_orig);
lpp.Add(gaze_right);
lpp.Add(pose_show_shoulderpitch);
lpp.Add(pose_show_orig1);
lpp.Add(pose_show_shouldroll);
lpp.Add(pose_show_orig2);
lpp.Add(pose_show_elbowroll);
lpp.Add(pose_show_elbowyaw1);
lpp.Add(pose_show_elbowyaw2);
lpp.Add(pose_show_wristyaw1);
lpp.Add(pose_show_wristyaw2);
lpp.Add(pose_show_orig3);
lpp.Add(pose_show_finger);
lpp.Add(pose_show_orig4);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double handheight = Parameters["HandHeight"].Value;
double fingerrigidness = Parameters["Finger"].Value;
double amplitude = Parameters["Amplitude"].Value;
double headupdown = Parameters["HeadUpDown"].Value;
double headleftright = Parameters["HeadLeftRight"].Value;
double shoulderPitch = 0;
double shoulderRollOut = 0;
double shoulderRollInw = 0;
double elbowYawOut = 0;
double elbowYawInw = 0;
double elbowRollOut = 0;
double elbowRollInw = 0;
double wristYawOut = 0;
double wristYawInw = 0;
double hand = 0;
double headpitch = 0;
double headyaw = 0;
//////////////////////////////////////////////////////////////////////////
shoulderPitch = -53.939 * handheight * handheight - 76.061 * handheight + 48.602;
hand = fingerrigidness; // *180D / Math.PI;
// Waving Mode 1
if (handheight <= 0.8 && handheight >= 0)
{
double shoulderRoll = -20.0;
// ShoulderRoll
double dSR;
if (amplitude <= 0.82) dSR = 10.0;
else dSR = 50.0 * amplitude - 31.0;
// waving pose pair
shoulderRollOut = shoulderRoll - dSR;
shoulderRollInw = shoulderRoll + dSR;
// ElbowYaw main joint for the waving
double elbowYaw;
if(handheight < 0.5) elbowYaw= 88.0;
else elbowYaw = -40.0 * handheight + 108.0;
double dEY;
if (amplitude <= 0.82) dEY = amplitude * 50.0 - 10.0;
else dEY = 31.0;
elbowYawOut = elbowYaw + dEY;
elbowYawInw = elbowYaw - dEY;
// Waving Mode 1
// WristYaw and ElbowRoll is computed
// Outward
double arg1 = elbowYawOut * Math.PI / 180D;
double arg2 = shoulderPitch * Math.PI / 180D;
double arg3 = shoulderRollOut * Math.PI / 180D;
Microsoft.FSharp.Math.Vector<double> resultOut
= NaoJointChain.Arm.OptPalmDirComp(arg1, arg2, arg3);
wristYawOut = resultOut[0] * 180 / Math.PI;
elbowRollOut = resultOut[1] * 180 / Math.PI;
// Inward
double arg4 = elbowYawInw * Math.PI / 180D;
double arg5 = shoulderRollInw * Math.PI / 180D;
Microsoft.FSharp.Math.Vector<double> resultInw
= NaoJointChain.Arm.OptPalmDirComp(arg4, arg2, arg5);
wristYawInw = resultInw[0] * 180 / Math.PI;
elbowRollInw = resultInw[1] * 180 / Math.PI;
System.Diagnostics.Debug.WriteLine("Waving mode 1 - WristYawOut: {0} ElbowRollOut: {1} WristYawInw: {2} ElbowRollInw: {3}",
wristYawOut, elbowRollOut, wristYawInw, elbowRollInw);
}
else // Waving Mode 2
{
double shoulderRoll;
if (handheight > 0.4) shoulderRoll = -20.0 * handheight - 12.0;
else shoulderRoll = -20.0;
// ShoulderRoll
double dSR;
if (amplitude <= 0.7) dSR = amplitude * 50.0 / 3.0 + 10.0 / 3.0;
else dSR = amplitude * 110.0 / 3.0 - 32.0 / 3.0;
// waving pose pair
shoulderRollOut = shoulderRoll - dSR;
shoulderRollInw = shoulderRoll + dSR;
// waving mode 2: ElbowRoll takes charge of the waving
double elbowRoll = 30.0;
double OutdER, InwdER;
if (amplitude > 0.7)
{
OutdER = amplitude * 80.0 / 3.0 + 4.0 / 3.0;
InwdER = 20;
}
else
{
InwdER = OutdER = amplitude * 100.0 / 3.0 - 10.0 / 3.0;
}
elbowRollInw = elbowRoll + InwdER;
elbowRollOut = elbowRoll - OutdER;
// waving mode 2: ElbowYaw and WristYaw will keep the hand facing forward
double elbowYaw = 0.0;
double dEY = 0.0;
elbowYawOut = elbowYaw + dEY;
elbowYawInw = elbowYaw - dEY;
// WristYaw and ElbowYaw is computed
wristYawOut = 0.0;
wristYawInw = 0.0;
//// Outward
//double arg1 = elbowYaw * Math.PI / 180D;
//double arg2 = elbowRollOut * Math.PI / 180D;
//double arg3 = shoulderPitch * Math.PI / 180D;
//double arg4 = shoulderRollOut * Math.PI / 180D;
//Microsoft.FSharp.Math.Vector<double> resultOut
// = NaoJointChain.Arm.OptPalmDirCompWY(arg1, arg2, arg3, arg4);
//wristYawOut = resultOut[0] * 180 / Math.PI;
//elbowYawOut = resultOut[1] * 180 / Math.PI;
//// Inward
////double arg5 = elbowYawInw * Math.PI / 180D;
//double arg6 = elbowRollInw * Math.PI / 180D;
//double arg7 = shoulderRollInw * Math.PI / 180D;
//Microsoft.FSharp.Math.Vector<double> resultInw
// = NaoJointChain.Arm.OptPalmDirCompWY(arg1, arg6, arg3, arg7);
//wristYawInw = resultInw[0] * 180 / Math.PI;
//elbowYawInw = resultInw[1] * 180 / Math.PI;
////Console.WriteLine("WristYawOut: {0} ElbowYawOut: {1} WristYawInw: {2} ElbowYawInw: {3}",
//// wristYawOut, elbowYawOut, wristYawInw, elbowYawInw);
//Console.WriteLine("WristYawOut: {0} WristYawInw: {1}",
// wristYawOut, wristYawInw);
System.Diagnostics.Debug.WriteLine("Waving mode 2");
}
// -35.0 ~ 25.0
headpitch = -5.0 * headupdown;
// -50 ~ 50
headyaw = 5.0 * headleftright;
Head head = new Head(false, headpitch, headyaw);
PoseProfile pose_head = new PoseProfile("OutSwing", head);
// Pose1
// frame1 outward
RArm out_arm = new RArm(false,shoulderPitch,shoulderRollOut,elbowYawOut,elbowRollOut,wristYawOut,hand);
PoseProfile pose_arm_out = new PoseProfile("OutSwing", out_arm, true);
// frame2 inward
RArm inw_arm = new RArm(false,shoulderPitch,shoulderRollInw,elbowYawInw,elbowRollInw,wristYawInw,hand);
PoseProfile pose_arm_inw = new PoseProfile("InwSwing", inw_arm, true);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_head);
lpp.Add(pose_arm_out);
lpp.Add(pose_arm_inw);
return lpp;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double width = Parameters["Width"].Value;
double height = Parameters["Height"].Value;
double amplitude = Parameters["AmplitudeFinger"].Value;
double palmdir = Parameters["PalmDir"].Value;
double headud = Parameters["HeadUpDown"].Value;
double headlr = Parameters["HeadLeftRight"].Value;
double wristyaw = 60 * palmdir + 30;
// width
double shldroll = -30 * width;
double elbyaw = 90;
// height
double shldpitch = -60 * height + 90;
double elbroll = -30 * height + 60;
double delta_elbroll = 50;
// finger open pose
double fingeropen = 1;
double elbrollopen = elbroll + delta_elbroll;
// finger close pose
double elbrollclose = elbroll;
double fingerclose = 0.3;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
// degree
double headYaw;
if (headlr > 0.5)
{
headYaw = -40 * headlr + 20;
}
else
{
headYaw = double.NaN;
}
Head head = new Head(false, headPitch, headYaw);
RArm rarm_down_finger_open = new RArm(false, shldpitch, shldroll, elbyaw, elbrollopen, wristyaw, fingeropen);
RArm rarm_down_finger_close = new RArm(false, shldpitch, shldroll, elbyaw, elbrollclose, wristyaw, fingerclose);
// Joints
PoseProfile pose_head = new PoseProfile("OpenPose", head);
PoseProfile pose_rarm_down_finger_open = new PoseProfile("OpenPose", rarm_down_finger_open, true, 1, 0);
PoseProfile pose_rarm_down_finger_close = new PoseProfile("ClosePose", rarm_down_finger_close, true, 1, 0);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_head);
lpp.Add(pose_rarm_down_finger_open);
lpp.Add(pose_rarm_down_finger_close);
return lpp;
}
public override JointChain DeepCopy()
{
JointChain jc = new Head(true, this.HeadPitch, this.HeadYaw);
return jc;
}
protected override List<PoseProfile> CoreParameterToPose(bool right = true)
{
double width = Parameters["Width"].Value;
double upheight = Parameters["UpHeight"].Value;
double downheight = Parameters["DownHeight"].Value;
double amplitude = Parameters["Amplitude"].Value;
double palmdir = Parameters["PalmDirection"].Value;
double finger = Parameters["Finger"].Value;
double headud = Parameters["HeadUpDown"].Value;
// finger 0~1
double hand = 0.4 * finger + 0.6;
double wristyaw = 90 * palmdir;
// Up pose center width
double up_shldroll = -30 * width;
double up_elbyaw = 90;
// Up pose center height
double up_shldpitch = -30 * upheight + 30;
double up_elbroll = 88.5;
// Up pose palm dir
double up_wristyaw = wristyaw;
double up_hand = hand;
// Down pose center width
double dp_shldroll = -30 * width;
double dp_elbyaw = 90;
// Down pose center height
double dp_shldpitch = -50 * downheight + 90;
double dp_elbroll = -20 * downheight + 60;
// Down pose palm dir
double dp_wristyaw = wristyaw;
double dp_hand = 0.6;
double delta_shldroll = 10 * amplitude + 10;
double delta_elbyaw = 10 * amplitude + 19.5;
double delta_elbroll = delta_shldroll / 2;
// down pose left (to robot)
double dpl_shldroll = dp_shldroll + delta_shldroll;
double dpl_elbyaw = dp_elbyaw - delta_elbyaw;
double dpl_shldpitch = dp_shldpitch;
double dpl_elbroll = dp_elbroll + delta_elbroll;
double dpl_wristyaw = dp_wristyaw;
double dpl_hand = hand;
double headYawL = 10 * amplitude + 5;
// down pose right (to robot)
double dpr_shldroll = dp_shldroll - delta_shldroll;
double dpr_elbyaw = dp_elbyaw + delta_elbyaw;
double dpr_shldpitch = dp_shldpitch;
double dpr_elbroll = dp_elbroll - delta_elbroll;
double dpr_wristyaw = dp_wristyaw;
double dpr_hand = hand;
double headYawR = -10 * amplitude - 15;
// degree
double headPitch = base.NormalizeHeadPitch(headud);
RArm rarm_up = new RArm(false, up_shldpitch, up_shldroll, up_elbyaw, up_elbroll, up_wristyaw, up_hand);
RArm rarm_down = new RArm(false, dp_shldpitch, dp_shldroll, dp_elbyaw, dp_elbroll, dp_wristyaw, dp_hand);
RArm rarm_down_left = new RArm(false, dpl_shldpitch, dpl_shldroll, dpl_elbyaw, dpl_elbroll, dpl_wristyaw, dpl_hand);
Head headL = new Head(false, headPitch, headYawL);
RArm rarm_down_right = new RArm(false, dpr_shldpitch, dpr_shldroll, dpr_elbyaw, dpr_elbroll, dpr_wristyaw, dpr_hand);
Head headR = new Head(false, headPitch, headYawR);
// Joints
PoseProfile pose_rarm_up = new PoseProfile("UpPose", rarm_up);
PoseProfile pose_rarm_down = new PoseProfile("DownPose", rarm_down);
PoseProfile pose_rarm_downL = new PoseProfile("DownPoseLeft", rarm_down_left);
PoseProfile pose_head_l = new PoseProfile("DownPoseLeft", headL);
PoseProfile pose_rarm_downR = new PoseProfile("DownPoseRight", rarm_down_right);
PoseProfile pose_head_r = new PoseProfile("DownPoseRight", headR);
List<PoseProfile> lpp = new List<PoseProfile>();
lpp.Add(pose_rarm_up);
lpp.Add(pose_rarm_down);
lpp.Add(pose_rarm_downL);
lpp.Add(pose_head_l);
lpp.Add(pose_rarm_downR);
lpp.Add(pose_head_r);
return lpp;
}