public void update(NaoSkeleton skeleton)
{
// Given 0 to rotational angle for now
if (!skeleton.isUpperBody)
{
NaoPosition nao_pos = skeleton.Position;
Console.Out.WriteLine("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
Console.WriteLine("Walking to x: " + -nao_pos.Z + " y: " + -nao_pos.X);
proxy.walkTo(-nao_pos.Z, -nao_pos.X, 0.0f);
}
else
{
List<String> angleTypes = new List<String>() {"LShoulderPitch", "RShoulderPitch",
"LShoulderRoll", "RShoulderRoll",
"LElbowYaw", "RElbowYaw",
"LElbowRoll", "RElbowRoll"};
List<float> angles = new List<float>() { skeleton.LeftShoulder.Pitch, skeleton.RightShoulder.Pitch,
skeleton.LeftShoulder.Roll, skeleton.RightShoulder.Roll,
skeleton.LeftElbow.Yaw, skeleton.RightElbow.Yaw,
skeleton.LeftElbow.Roll, skeleton.RightElbow.Roll};
proxy.setAngles(angleTypes, angles, SPEED);
/* //Shoulder Angles
proxy.setAngles("LShoulderPitch", (float)skeleton.LeftShoulder.Pitch, SPEED);
proxy.setAngles("RShoulderPitch", (float)skeleton.RightShoulder.Pitch, SPEED);
proxy.setAngles("LShoulderRoll", (float)skeleton.LeftShoulder.Roll, SPEED);
proxy.setAngles("RShoulderRoll", (float)skeleton.RightShoulder.Roll, SPEED);
//Elbow Angles
proxy.setAngles("LElbowYaw", (float)skeleton.LeftElbow.Yaw, SPEED);
proxy.setAngles("RElbowYaw", (float)skeleton.RightElbow.Yaw, SPEED);
proxy.setAngles("LElbowRoll", (float)skeleton.LeftElbow.Roll, SPEED);
proxy.setAngles("RElbowRoll", (float)skeleton.RightElbow.Roll, SPEED);*/
//Shoulder Angles
Console.Out.WriteLine("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
Console.Out.WriteLine("LShoulderPitch: " + (float)skeleton.LeftShoulder.Pitch);
Console.Out.WriteLine("RShoulderPitch: " + (float)skeleton.RightShoulder.Pitch);
Console.Out.WriteLine("LShoulderRoll: " + (float)skeleton.LeftShoulder.Roll);
Console.Out.WriteLine("RShoulderRoll: " + (float)skeleton.RightShoulder.Roll);
//Elbow Angles
Console.Out.WriteLine("LElbowYaw: " + (float)skeleton.LeftElbow.Yaw);
Console.Out.WriteLine("RElbowYaw: " + (float)skeleton.RightElbow.Yaw);
Console.Out.WriteLine("LElbowRoll: " + (float)skeleton.LeftElbow.Roll);
Console.Out.WriteLine("RElbowRoll: " + (float)skeleton.RightElbow.Roll);
//Wrist Angles
//proxy.setAngles("LWristYaw", (float)skeleton.LeftWrist.getYaw(), SPEED);
//proxy.setAngles("RWristYaw", (float)skeleton.RightWrist.getYaw(), SPEED);
//Hand
//proxy.setAngles("LHand", (float)skeleton.LeftHand.isOpen(), SPEED);
//proxy.setAngles("RHand", (float)skeleton.RightHand.isOpen(), SPEED);
}
}
public void update(NaoSkeleton skeleton)
{
// Given 0 to rotational angle for now
NaoPosition nao_pos = skeleton.Position;
NaoPosition nao_pos_prv = lastUpdate.Position;
proxy.walkTo(nao_pos.X - nao_pos_prv.X, nao_pos.Y - nao_pos_prv.Y, 0.0f);
String[] angleTypes = {"LShoulderPitch", "RShoulderPitch",
"LShoulderRoll", "RShoulderRoll",
"LElbowYaw", "RElbowYaw",
"LElbowRoll", "RElbowRoll"};
float[] angles = {skeleton.LeftShoulder.Pitch, skeleton.RightShoulder.Pitch,
skeleton.LeftShoulder.Roll, skeleton.RightShoulder.Roll,
skeleton.LeftElbow.Yaw, skeleton.RightElbow.Yaw,
skeleton.LeftElbow.Roll, skeleton.RightElbow.Roll};
proxy.angleInterpolationWithSpeed(angleTypes, angles, SPEED);
/*
//Shoulder Angles
proxy.setAngles("LShoulderPitch", (float)skeleton.LeftShoulder.Pitch, SPEED);
proxy.setAngles("RShoulderPitch", (float)skeleton.RightShoulder.Pitch, SPEED);
proxy.setAngles("LShoulderRoll", (float)skeleton.LeftShoulder.Roll, SPEED);
proxy.setAngles("RShoulderRoll", (float)skeleton.RightShoulder.Roll, SPEED);
//Elbow Angles
proxy.setAngles("LElbowYaw", (float)skeleton.LeftElbow.Yaw, SPEED);
proxy.setAngles("RElbowYaw", (float)skeleton.RightElbow.Yaw, SPEED);
proxy.setAngles("LElbowRoll", (float)skeleton.LeftElbow.Roll, SPEED);
proxy.setAngles("RElbowRoll", (float)skeleton.RightElbow.Roll, SPEED);*/
//Shoulder Angles
Console.Out.WriteLine("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
Console.Out.WriteLine("LShoulderPitch: " + (float)skeleton.LeftShoulder.Pitch);
Console.Out.WriteLine("RShoulderPitch: " + (float)skeleton.RightShoulder.Pitch);
Console.Out.WriteLine("LShoulderRoll: " + (float)skeleton.LeftShoulder.Roll);
Console.Out.WriteLine("RShoulderRoll: " + (float)skeleton.RightShoulder.Roll);
//Elbow Angles
Console.Out.WriteLine("LElbowYaw: " + (float)skeleton.LeftElbow.Yaw);
Console.Out.WriteLine("RElbowYaw: " + (float)skeleton.RightElbow.Yaw);
Console.Out.WriteLine("LElbowRoll: " + (float)skeleton.LeftElbow.Roll);
Console.Out.WriteLine("RElbowRoll: " + (float)skeleton.RightElbow.Roll);
//Wrist Angles
//proxy.setAngles("LWristYaw", (float)skeleton.LeftWrist.getYaw(), SPEED);
//proxy.setAngles("RWristYaw", (float)skeleton.RightWrist.getYaw(), SPEED);
//Hand
//proxy.setAngles("LHand", (float)skeleton.LeftHand.isOpen(), SPEED);
//proxy.setAngles("RHand", (float)skeleton.RightHand.isOpen(), SPEED);
}
public NaoController(string ip)
{
try
{
this.ip = ip;
lastUpdate = null;
proxy = new MotionProxy(ip, 9559);
this.setStiffness(1.0f);
}
catch (Exception e)
{
Console.Out.WriteLine("HeadPanning.Connect exception: " + e);
}
}
public static ActionLibrary load(String path)
{
/*ActionLibrary al = new ActionLibrary();
Stream stream = File.Open(path, FileMode.Open);
BinaryFormatter bformatter = new BinaryFormatter();
al = (ActionLibrary)bformatter.Deserialize(stream);
stream.Close();
return al;*/
Dictionary<string, string> actions = new Dictionary<string, string>();
actions["raise the roof"] = "raisetheroof";
actions["walk forward"] = "walkforward";
actions["walk left"] = "walkleft";
actions["walk back"] = "walkbackward";
actions["walk right"] = "walkright";
actions["wave right"] = "waveright";
actions["wave left"] = "waveleft";
string filename = "Z:/dev/kinect-nao/recordings/";
ActionLibrary lib = new ActionLibrary();
foreach (string actionName in actions.Keys)
{
List<HumanSkeleton> seq = new List<HumanSkeleton>();
using (StreamReader s = new StreamReader(filename + actions[actionName] + "/"
+ actionName + "1.rec"))
{
while (!s.EndOfStream)
{
seq.Add(new HumanSkeleton(s.ReadLine()));
}
}
ActionSequence<NaoSkeleton> naoSeq = new ActionSequence<NaoSkeleton>();
foreach (HumanSkeleton h in seq)
{
AngleConverter ac = new AngleConverter(h);
naoSeq.append(ac.getNaoSkeleton());
}
if (actionName.StartsWith("walk"))
{
NaoSkeleton start = naoSeq.get(0);
NaoSkeleton end = naoSeq.get(naoSeq.size() - 1);
NaoSkeleton diff = new NaoSkeleton(new NaoPosition(end.Position.X - start.Position.X, end.Position.Y - start.Position.Y, end.Position.Z - start.Position.Z));
naoSeq = new ActionSequence<NaoSkeleton>();
naoSeq.append(diff);
}
lib.appendToCache(naoSeq);
lib.setCachedName(actionName);
lib.saveCache();
}
return lib;
}