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;

            double lsrA = 5 * width + 25;
            double rsrA = 8 * width + 10;
            double lerA = -30;
            double rerA = 30;
            double lspA = -20 * height + 40;
            double rspA = -15 * height + 40;
            double leyA = -105*palmdir + 15;
            double reyA = 60 * palmdir - 15;
            double lwyA = -90 * palmdir;
            double rwyA = 104 * palmdir;

            double lsrB = -8 * width - 10;
            double rsrB = -10 * width - 10;
            double lerB = -15 * width - 30;
            double rerB = 30;
            double lspB = -20 * height + 40;
            double rspB = -20 * height + 40;
            double leyB = -45 * palmdir + 15;
            double reyB = 90*palmdir;
            double lwyB = -104 * palmdir;
            double rwyB = 90 * palmdir;

            // finger 0~1
            double hand = 0.4 * finger + 0.6;
            // degree
            double headPitch = base.NormalizeHeadPitch(headud);
            // degree
            double headYaw = 30.0 * headlr;

            LArm larmA = new LArm(false, lspA, lsrA, leyA, lerA, lwyA, hand);
            RArm rarmA = new RArm(false, rspA, rsrA, reyA, rerA, rwyA, hand);
            LArm larmB = new LArm(false, lspB, lsrB, leyB, lerB, lwyB, hand);
            RArm rarmB = new RArm(false, rspB, rsrB, reyB, rerB, rwyB, hand);
            Head head = new Head(false, headPitch, headYaw);

            // Joints
            PoseProfile pose_A_arm_l = new PoseProfile("FromA", larmA);
            PoseProfile pose_A_arm_r = new PoseProfile("FromA", rarmA);
            PoseProfile pose_A_head = new PoseProfile("FromA", head);
            PoseProfile pose_B_arm_l = new PoseProfile("ToB", larmB);
            PoseProfile pose_B_arm_r = new PoseProfile("ToB", rarmB);

            List<PoseProfile> lpp = new List<PoseProfile>();
            lpp.Add(pose_A_arm_l);
            lpp.Add(pose_A_arm_r);
            lpp.Add(pose_A_head);
            lpp.Add(pose_B_arm_l);
            lpp.Add(pose_B_arm_r);

            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;
        }
        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)
        {
            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 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 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 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 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;
        }
示例#19
0
        /// <summary>
        /// 
        /// </summary>
        /// <returns></returns>
        public override JointChain MirrorLeftRight() 
        {
            Arm m_arm = null;           
            if (this.Name == "RArm")
            {
                m_arm = new LArm(                    
                    true, 
                    this.ShoulderPitch,
                    -this.ShoulderRoll,
                    -this.ElbowYaw,
                    -this.ElbowRoll,
                    -this.WristYaw,
                    this.Hand
                    );
            } 
            else if (this.Name == "LArm")
            {
                m_arm = new RArm(
                    true, 
                    this.ShoulderPitch,
                    -this.ShoulderRoll,
                    -this.ElbowYaw,
                    -this.ElbowRoll,
                    -this.WristYaw,
                    this.Hand
                    );
            }
            else
            {
                Debug.WriteLine("JointChainMirror: wrong arm joint chain name");
            }

            return m_arm; 
        }
        protected List<PoseProfile> StandArmPose(string mfname)
        {
            RArm armR = new RArm(
                false,
                86,
                -7,
                68,
                23.5,
                6,
                0.3);
            LArm armL = (LArm)armR.MirrorLeftRight();
            PoseProfile armppR = new PoseProfile(mfname, armR);
            PoseProfile armppL = new PoseProfile(mfname, armL);
            List<PoseProfile> lpp = new List<PoseProfile>();
            lpp.Add(armppR);
            lpp.Add(armppL);

            return lpp;
        }
        protected List<PoseProfile> ArmChest(string mfname)
        {
            RArm armR = new RArm(
                false,
                60,
                5,
                50,
                88.5,
                15,
                0);
            LArm armL = new LArm(
                false,
                70,
                -5,
                -40,
                -88.5,
                -15,
                 0);
            PoseProfile armppR = new PoseProfile(mfname, armR);
            PoseProfile armppL = new PoseProfile(mfname, armL);
            List<PoseProfile> lpp = new List<PoseProfile>();
            lpp.Add(armppR);
            lpp.Add(armppL);

            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 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 width      = Parameters["Width"].Value;
            double height     = Parameters["Height"].Value;
            double amplitude  = Parameters["Amplitude"].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;
            double wristyaw = 90 * palmdir;

            // pose center width
            double orig_shldroll  = 10 * width;
            // pose center height
            double orig_shldpitch = -40 * height + 70;
            double orig_elbroll   = -30 * height + 60;
            // Amplitude
            double orig_elbyaw = -10 * amplitude + 60;
            // pose palm direction
            double orig_wristyaw  = wristyaw;
            double orig_hand      = 0.4;

            double delta_shldroll  = 25 * amplitude + 15;
            //double delta_elbyaw  = 10 * amplitude + 59.5;
            // swing outward
            double out_shldroll    = orig_shldroll - delta_shldroll;
            double out_elbyaw      = 119.5; //orig_elbyaw + delta_elbyaw;
            double out_shlorigitch = orig_shldpitch;
            double out_elbroll     = orig_elbroll - delta_shldroll / 3;
            double out_wristyaw    = orig_wristyaw;
            double out_hand        = hand;

            // degree
            double headpitch = base.NormalizeHeadPitch(headud);
            // degree
            double headYaw = double.NaN;

            Head head = new Head(false, headpitch, headYaw);
            RArm rarm_orig = new RArm(false, orig_shldpitch, orig_shldroll, orig_elbyaw, orig_elbroll, orig_wristyaw, orig_hand);
            RArm rarm_out = new RArm(false, out_shlorigitch, out_shldroll, out_elbyaw, out_elbroll, out_wristyaw, out_hand);

            // Joints
            PoseProfile pose_head = new PoseProfile("UpPose", head);
            PoseProfile pose_rarm_orig = new PoseProfile("UpPose", rarm_orig, true);
            PoseProfile pose_rarm_out = new PoseProfile("DownPose", rarm_out, true);

            List<PoseProfile> lpp = new List<PoseProfile>();
            lpp.Add(pose_head);
            lpp.Add(pose_rarm_orig);
            lpp.Add(pose_rarm_out);

            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;
        }
        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 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;
        }
示例#29
0
 public override JointChain DeepCopy()
 {
     JointChain jc = new RArm(true, this.HipYawPitch, this.HipPitch, this.HipRoll, this.KneePitch, this.AnklePitch, this.AnkleRoll);
     return jc;
 }
示例#30
0
 public override JointChain DeepCopy()
 {
     JointChain jc = new RArm(true, this.ShoulderPitch, this.ShoulderRoll, this.ElbowYaw, this.ElbowRoll, this.WristYaw, this.Hand);
     return jc;
 }