static void ExecuteOnNao(List<float[]> naoSeq, bool isJointAction)
{
MotionProxy mp = new MotionProxy("127.0.0.1", 9559);
// do stuff with naoskeletons and real NAO
foreach (float[] act in naoSeq) {
float LShoulderRoll = act[(int)AngleConverter.NaoJointAngle.LShoulderRoll];
float LShoulderPitch = act[(int)AngleConverter.NaoJointAngle.LShoulderPitch];
float LElbowYaw = act[(int)AngleConverter.NaoJointAngle.LElbowYaw];
float LElbowRoll = act[(int)AngleConverter.NaoJointAngle.LElbowRoll];
Console.WriteLine("LShoulderRoll: " + LShoulderRoll);
Console.WriteLine("LShoulderPitch: " + LShoulderPitch);
/* mp.setAngles("LElbowYaw", LElbowYaw, 0.4f);
mp.setAngles("LElbowRoll", LElbowRoll, 0.4f);
mp.setAngles("LShoulderRoll", LShoulderRoll, 0.4f);
mp.setAngles("LShoulderPitch", LShoulderPitch, 0.4f);*/
String[] joints = new String[(int)AngleConverter.NaoJointAngle.count];
joints[(int)AngleConverter.NaoJointAngle.LElbowRoll] = "LElbowRoll";
joints[(int)AngleConverter.NaoJointAngle.LElbowYaw] = "LElbowYaw";
joints[(int)AngleConverter.NaoJointAngle.LShoulderPitch] = "LShoulderPitch";
joints[(int)AngleConverter.NaoJointAngle.LShoulderRoll] = "LShoulderRoll";
joints[(int)AngleConverter.NaoJointAngle.RElbowRoll] = "RElbowRoll";
joints[(int)AngleConverter.NaoJointAngle.RElbowYaw] = "RElbowYaw";
joints[(int)AngleConverter.NaoJointAngle.RShoulderPitch] = "RShoulderPitch";
joints[(int)AngleConverter.NaoJointAngle.RShoulderRoll] = "RShoulderRoll";
mp.angleInterpolationWithSpeed(joints, act, 0.3f);
System.Threading.Thread.Sleep(50);
}
Console.Read();
}
/// <summary>
/// Make the Nao kneel, with specified depth.
/// </summary>
/// <param name="depth">0 means standing. 1 means sitting.</param>
public void Kneel(float depth)
{
ValidateProxies();
if (depth > 1f)
{
depth = 1f;
}
if (depth < 0f)
{
depth = 0;
}
ArrayList angles = new ArrayList();
angles.Add(-depth);//low
angles.Add(-depth);
angles.Add(depth + depth);//high
angles.Add(depth * 2);
angles.Add(-depth);//low
angles.Add(-depth);
motion.angleInterpolationWithSpeed(kneelNames, angles, 0.3F);
}
public void SpreadArms()
{
motion.angleInterpolationWithSpeed(spreadArmsNames, spreadArmsAngles, grabSpeed);
}