// return the a NaoSkeleton with given joints
public NaoSkeleton getNaoSkeleton()
{
Joint center = hk.getJoint(JointID.HipCenter);
NaoPosition position = new NaoPosition((float)center.x, (float)center.y, (float)center.z);
NaoElbow LElbow = new NaoElbow(this.angles[(int)NaoJointAngle.LElbowYaw], this.angles[(int)NaoJointAngle.LElbowRoll], true);
NaoElbow RElbow = new NaoElbow(this.angles[(int)NaoJointAngle.RElbowYaw], this.angles[(int)NaoJointAngle.RElbowRoll], false);
NaoShoulder LShoulder = new NaoShoulder(this.angles[(int)NaoJointAngle.LShoulderPitch], this.angles[(int)NaoJointAngle.LShoulderRoll], true);
NaoShoulder RShoulder = new NaoShoulder(this.angles[(int)NaoJointAngle.RShoulderPitch], this.angles[(int)NaoJointAngle.RShoulderRoll], false);
NaoWrist LWrist = new NaoWrist(0);
NaoWrist RWrist = new NaoWrist(0);
NaoHand LHand = new NaoHand(true);
NaoHand RHand = new NaoHand(true);
return new NaoSkeleton(position, LShoulder, RShoulder, LWrist, RWrist, LElbow, RElbow, LHand, RHand);
}
public NaoSkeleton(NaoPosition position,
NaoShoulder leftShoulder, NaoShoulder rightShoulder,
NaoWrist leftWrist, NaoWrist rightWrist,
NaoElbow leftElbow, NaoElbow rightElbow,
NaoHand leftHand, NaoHand rightHand)
{
this.isUpperBody = true;
this.Position = position;
this.LeftShoulder = leftShoulder;
this.RightShoulder = rightShoulder;
this.LeftWrist = leftWrist;
this.RightWrist = rightWrist;
this.LeftElbow = leftElbow;
this.RightElbow = rightElbow;
this.LeftHand = leftHand;
this.RightHand = rightHand;
}