public bool connect(string ip, bool onlymotion = false) { this.IP = ip; bool b = false; try { proxyMotion = new MotionProxy(ip, Port); if (onlymotion == false) { bool bc = proxyMotion.setCollisionProtectionEnabled("Arms", false); if (!bc) Console.WriteLine("Failed to disable collision protection"); StartMotorMonitor(); proxyBehaviorManager = new BehaviorManagerProxy(ip, Port); // Turn off the LEDs proxyLed = new LedsProxy(ip, Port); List<string> ledgroups = proxyLed.listGroups(); foreach (string ledgroup in ledgroups) { proxyLed.off(ledgroup); } // Speech proxyTTS = new TextToSpeechProxy(ip, Port); if (ip != "127.0.0.1") { // Sentinel proxySentinel = new SentinelProxy(ip, Port); // Turning off "Motor hot!" proxySentinel.enableHeatMonitoring(false); } // camera proxyCamera = new VideoDeviceProxy(ip, Port); // IdleMovement InitIdleMovement(); } b = true; } catch (Exception e) { Console.WriteLine("MotionProxy.Connect Exception: " + e); b = false; } IsConnected = b; return b; }
private void ChangePositionFromBackToSitDown() { var bodyPart = NAOBodyParts.Arms.ToString(); motionProxy.setCollisionProtectionEnabled(bodyPart, false); List <string> names = new List <string>(); List <float[]> times = new List <float[]>(); List <float[]> keys = new List <float[]>(); names.Add("HeadYaw"); times.Add(new[] { 1.00000f, 2.00000f, 2.80000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f }); keys.Add(new[] { -0.02459f, -0.00000f, 0.04138f, 0.00000f, -0.00618f, 0.01376f, 0.02450f, -0.02305f }); names.Add("HeadPitch"); times.Add(new[] { 1.00000f, 2.00000f, 2.80000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f }); keys.Add(new[] { 0.06285f, 0.00000f, 0.51490f, -0.67200f, -0.02152f, 0.19478f, 0.07359f, -0.03532f }); names.Add("LShoulderPitch"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 5.90000f, 6.20000f }); keys.Add(new[] { 2.08560f, 2.09440f, 2.03865f, 2.08560f, 2.08567f, 1.30846f, 0.90348f, 0.70337f, 0.90195f }); names.Add("LShoulderRoll"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f }); keys.Add(new[] { 0.98018f, 0.36652f, 0.34907f, 0.34907f, 0.40143f, 1.17193f, 0.39726f, 0.21625f }); names.Add("LElbowYaw"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f }); keys.Add(new[] { -0.76091f, 0.15708f, 0.12268f, 0.08727f, 0.05978f, -0.95419f, -0.96339f, -0.43877f }); names.Add("LElbowRoll"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 4.60000f, 5.00000f, 5.60000f, 6.20000f }); keys.Add(new[] { -0.62430f, 0.00000f, -1.54462f, -1.56210f, -0.87441f, -1.03498f, -0.67952f, -1.12285f, -1.23483f }); names.Add("LWristYaw"); times.Add(new[] { 1.00000f, 2.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { -0.70568f, -1.78868f, -0.71949f, -0.04760f, 0.05978f }); names.Add("LHand"); times.Add(new[] { 1.00000f, 2.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { 0.00088f, 0.00133f, 0.00158f, 0.00022f, 0.00524f }); names.Add("RShoulderPitch"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 5.90000f, 6.20000f }); keys.Add(new[] { 2.08560f, 2.09440f, 1.95283f, 2.08560f, 2.08567f, 1.91908f, 1.03703f, 0.72955f, 0.92504f }); names.Add("RShoulderRoll"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f }); keys.Add(new[] { -1.07384f, -0.36652f, -0.34907f, -0.34907f, -0.41015f, -0.89897f, -0.74096f, -0.24241f }); names.Add("RElbowYaw"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f }); keys.Add(new[] { 0.76542f, -0.15708f, -0.25775f, -0.08727f, -0.06600f, 1.57691f, 1.06762f, 0.52459f }); names.Add("RElbowRoll"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 4.60000f, 5.00000f, 5.60000f, 6.20000f }); keys.Add(new[] { 0.71028f, 0.00000f, 1.54462f, 1.56210f, 0.71384f, 1.05418f, 0.95726f, 1.14594f, 1.27019f }); names.Add("RWristYaw"); times.Add(new[] { 1.00000f, 2.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { 1.15199f, 1.72571f, 1.11057f, 0.24693f, -0.05680f }); names.Add("RHand"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { 0.00000f, 0.00000f, 0.00032f, 0.00005f, 0.00221f, 0.00523f }); names.Add("LHipYawPitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { -0.47090f, 0.00000f, 0.00771f, -0.00000f, -0.65498f, -0.70407f, -0.61816f }); names.Add("LHipRoll"); times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { 0.04453f, 0.00000f, 0.05833f, 0.00000f, -0.06439f, 0.14083f, 0.27923f }); names.Add("LHipPitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { 0.25008f, -0.17453f, 0.37434f, -0.17453f, -1.17807f, -1.57080f, -1.57998f }); names.Add("LKneePitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { 0.09200f, 1.67552f, 2.02024f, 1.67552f, 1.07683f, 1.40161f, 1.39590f }); names.Add("LAnklePitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { 0.82525f, 0.24435f, -0.45411f, 0.24435f, 0.67799f, 0.77609f, 0.83599f }); names.Add("LAnkleRoll"); times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { -0.03371f, 0.00000f, -0.03371f, 0.00000f, 0.17032f, 0.09083f, -0.00916f }); names.Add("RHipRoll"); times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { -0.14876f, 0.00000f, -0.02297f, 0.00000f, 0.06140f, -0.12200f, -0.26841f }); names.Add("RHipPitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { 0.18557f, -0.17453f, 0.28221f, -0.17453f, -1.16435f, -1.57080f, -1.59233f }); names.Add("RKneePitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { 0.19179f, 1.67552f, 2.02645f, 1.67552f, 1.09532f, 1.37682f, 1.40825f }); names.Add("RAnklePitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { 0.46638f, 0.24435f, -0.37425f, 0.24435f, 0.71795f, 0.76985f, 0.84988f }); names.Add("RAnkleRoll"); times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f }); keys.Add(new[] { 0.16725f, 0.00000f, -0.03984f, 0.00000f, -0.14415f, -0.11992f, 0.01845f }); ChangePosition(names, times, keys); motionProxy.setCollisionProtectionEnabled(bodyPart, true); }
private void ChangePositionFromBackToStand() { var bodyPart = NAOBodyParts.Arms.ToString(); motionProxy.setCollisionProtectionEnabled(bodyPart, false); List <string> names = new List <string>(); List <float[]> times = new List <float[]>(); List <float[]> keys = new List <float[]>(); names.Add("HeadYaw"); times.Add(new[] { 1.00000f, 2.00000f, 2.80000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { -0.02459f, -0.00000f, 0.04138f, 0.00000f, 0.00000f, 0.00000f, -0.51393f, -0.30224f, 0.26354f, 0.12043f, -0.00940f, 0.06592f }); names.Add("HeadPitch"); times.Add(new[] { 1.00000f, 2.00000f, 2.80000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 0.06285f, 0.00000f, 0.51490f, -0.67200f, 0.00000f, 0.34907f, 0.17177f, 0.34511f, 0.01047f, 0.02967f, 0.37886f, -0.03993f }); names.Add("LShoulderPitch"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 2.08560f, 2.08567f, 2.03865f, 2.08560f, 2.08560f, 2.08560f, 2.03251f, 1.28698f, 0.95717f, 0.73321f, 0.73321f, 1.59225f }); names.Add("LShoulderRoll"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.00000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 0.98018f, 0.36652f, 0.34907f, 0.34907f, 0.36652f, 0.00870f, 0.29909f, 0.44331f, 0.07052f, 0.26687f, 0.49851f, 0.49851f, 0.21932f }); names.Add("LElbowYaw"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { -0.76091f, 0.15708f, 0.12268f, 0.08727f, 0.08727f, 0.08727f, -1.96049f, -0.85448f, -0.25008f, 0.08308f, 0.08308f, -1.03089f }); names.Add("LElbowRoll"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.20000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { -0.62430f, -0.03491f, -1.54462f, -1.56210f, -0.69813f, -0.00870f, -0.19199f, -1.04615f, -0.87280f, -0.82985f, -0.80535f, -0.80535f, -0.68105f }); names.Add("LWristYaw"); times.Add(new[] { 1.00000f, 2.60000f, 5.60000f, 6.50000f, 7.40000f, 10.50000f }); keys.Add(new[] { -0.70568f, -1.78868f, -0.72562f, -0.71949f, -0.72256f, -0.71182f }); names.Add("LHand"); times.Add(new[] { 1.00000f, 2.60000f, 5.60000f, 6.50000f, 7.40000f, 10.50000f }); keys.Add(new[] { 0.00088f, 0.00133f, 0.00219f, 0.00269f, 0.00290f, 0.00451f }); names.Add("RShoulderPitch"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 2.08560f, 2.08567f, 1.95283f, 2.08560f, 2.08560f, 2.08560f, 2.07708f, 2.03720f, 1.44047f, 0.89131f, 0.89131f, 1.46961f }); names.Add("RShoulderRoll"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { -1.07384f, -0.36652f, -0.34907f, -0.34907f, -0.36652f, -0.00870f, -0.45717f, -0.56455f, -0.70568f, -0.87266f, -0.68068f, -0.15037f }); names.Add("RElbowYaw"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 0.76542f, -0.15708f, -0.25775f, -0.08727f, -0.08727f, -0.08727f, -0.08134f, -0.06907f, -0.02612f, 0.00062f, 0.00062f, 0.91269f }); names.Add("RElbowRoll"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 0.71028f, 0.03491f, 1.54462f, 1.56210f, 0.69813f, 0.00870f, 0.09668f, 0.02152f, 0.19026f, 0.45379f, 0.55995f, 0.38047f }); names.Add("RWristYaw"); times.Add(new[] { 1.00000f, 2.60000f, 5.60000f, 6.50000f, 7.40000f, 10.50000f }); keys.Add(new[] { 1.15199f, 1.72571f, 1.07989f, 1.07836f, 1.07989f, 1.08450f }); names.Add("RHand"); times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 4.30000f, 5.60000f, 6.50000f, 7.40000f, 10.50000f }); keys.Add(new[] { 0.00000f, 0.00000f, 0.00032f, 0.00000f, 0.00287f, 0.00300f, 0.00308f, 0.00376f }); names.Add("LHipYawPitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { -0.47090f, 0.00000f, 0.00771f, -0.00000f, -0.65498f, -0.49851f, -0.85900f, -0.69639f, -0.40225f, -0.40225f, -0.23006f }); names.Add("LHipRoll"); times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 0.04453f, 0.00000f, 0.05833f, 0.00000f, -0.06478f, 0.54105f, 0.15498f, -0.17483f, 0.00925f, 0.19199f, 0.17453f, 0.15191f }); names.Add("LHipPitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 0.25008f, -0.17453f, 0.37434f, -0.17453f, -1.17808f, -1.57080f, -0.85746f, -0.05672f, -0.45556f, -0.85521f, -0.83599f, 0.21327f }); names.Add("LKneePitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 0.09200f, 1.67552f, 2.02024f, 1.67552f, 1.07777f, 1.67552f, 2.11075f, 2.11227f, 2.11253f, 2.11253f, 2.11253f, -0.08901f }); names.Add("LAnklePitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 0.82525f, 0.24435f, -0.45411f, 0.24435f, 0.67748f, 0.66323f, -0.45564f, -1.10145f, -1.18952f, -1.18952f, -1.18952f, 0.08126f }); names.Add("LAnkleRoll"); times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { -0.03371f, 0.00000f, -0.03371f, 0.00000f, 0.17109f, -0.10472f, -0.39573f, -0.09967f, 0.04299f, 0.11810f, 0.08727f, -0.13648f }); names.Add("RHipRoll"); times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { -0.14876f, 0.00000f, -0.02297f, 0.00000f, 0.06190f, -0.54105f, -0.55833f, -0.61049f, -0.62657f, -0.29671f, -0.01745f, -0.06285f }); names.Add("RHipPitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 0.18557f, -0.17453f, 0.28221f, -0.17453f, -1.16456f, -1.57080f, -1.52484f, -1.56012f, -1.02974f, -0.90583f, -0.90583f, 0.20858f }); names.Add("RKneePitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 0.19179f, 1.67552f, 2.02645f, 1.67552f, 1.09386f, 1.67552f, 1.22111f, 1.09992f, 1.08385f, 0.87616f, 1.76278f, -0.07666f }); names.Add("RAnklePitch"); times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 0.46638f, 0.24435f, -0.37425f, 0.24435f, 0.71737f, 0.66323f, 0.78545f, 0.78392f, 0.44157f, 0.40317f, -0.57945f, 0.08134f }); names.Add("RAnkleRoll"); times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f }); keys.Add(new[] { 0.16725f, 0.00000f, -0.03984f, 0.00000f, -0.16084f, 0.10472f, 0.00925f, -0.09813f, 0.44331f, 0.67960f, 0.27751f, 0.06294f }); ChangePosition(names, times, keys); motionProxy.setCollisionProtectionEnabled(bodyPart, true); }