private void CreateLocalLists()
{
foreach (Robot robot in BaseFile.Robots)
{
Robots.Add(robot);
}
foreach (Weapon weapon in BaseFile.Weapons)
{
Weapons.Add(weapon);
}
foreach (Polymodel model in BaseFile.Models)
{
Models.Add(model);
}
foreach (JointPos joint in BaseFile.Joints)
{
Joints.Add(joint);
}
foreach (ushort bm in BaseFile.ObjBitmaps)
{
ObjBitmaps.Add(bm);
}
foreach (ushort bm in BaseFile.ObjBitmapPointers)
{
ObjBitmapPointers.Add(bm);
}
}
[Fact] public void Average()
{
Data_Generator dg = new Data_Generator();
Hand_Generator hg = new Hand_Generator(dg);
int dat_len = dg.newInt(100);
List <Joints> dat_jL = hg.newJointsList(dat_len);
Joints exp_ave = hg.newJoints();
Mock <IJointsHelper> mock_jh = new Mock <IJointsHelper>();
for (int i = 0; i < dat_len; i++)
{
mock_jh.Setup(m => m
.add(It.IsAny <Joints>(), dat_jL[i]))
.Returns(exp_ave);
}
mock_jh.Setup(m => m
.div(exp_ave, dat_len))
.Returns(exp_ave);
Stats s = new Stats(mock_jh.Object);
Joints act_ave = s.average(dat_jL);
mock_jh.Verify(m => m.add(It.IsAny <Joints>(), It.IsAny <Joints>()), Times.Exactly(dat_len));
mock_jh.Verify(m => m.div(exp_ave, dat_len), Times.Once());
test.jointsEqual(exp_ave, act_ave);
}
/// <summary>
/// Adds the specified joint to the collection.
/// </summary>
/// <param name="joint">The joint type.</param>
/// <param name="radius">The size of the ellipse</param>
/// <param name="brush">The brush used to fill the ellipse.</param>
public void AddJoint(JointType joint, double radius, Brush brush)
{
Brush HAND_BRUSH = new SolidColorBrush(Colors.Green);
if (joint == JointType.HandTipLeft || joint == JointType.HandTipRight)
{
Joints.Add(joint, new Ellipse
{
Width = radius * 6,
Height = radius * 6,
Fill = HAND_BRUSH
});
}
else if (joint != JointType.ThumbLeft && joint != JointType.ThumbRight)
{
Joints.Add(joint, new Ellipse
{
Width = 0,
Height = 0,
Fill = HAND_BRUSH
});
}
else
{
Joints.Add(joint, new Ellipse
{
Width = radius * 3,
Height = radius * 3,
Fill = brush
});
}
}
public Joint GetJoint(String name)
{
Joint joint = null;
Joints.TryGetValue(name, out joint);
return(joint as Joint);
}
public void Remove(IGameObject obj)
{
if (!(obj is PhysicsObject))
{
throw new NotImplementedException("Currently only PhysicsObjects can be added to a structure.");
}
PhysicsObject physObj = (PhysicsObject)obj;
if (!objects.Contains(physObj))
{
return;
}
physObj.ParentStructure = null;
physObj.CollisionIgnorer = null;
physObj.CollisionIgnoreGroup = 0;
physObj.Collided -= this.OnCollided;
foreach (var joint in Joints.FindAll(j => j.Object1 == physObj || j.Object2 == physObj))
{
joint.Destroy();
PhysicsGameBase.Instance.Remove(joint);
}
objects.Remove(physObj);
CalculateMomentOfInertia();
}
public TrackingStateGait getJointPosition(JointTypeGait joint, bool checkTracked, out float X, out float Y, out float Z)
{
if (Joints == null)
{
X = missingX;
Y = missingY;
Z = missingZ;
return(TrackingStateGait.NotTracked);
}
if (checkTracked)
{
int s = Joints.Select(x => (x.TrackingState == TrackingStateGait.Tracked) ? 0 : 1).Sum();
if (s > 5)
{
X = missingX;
Y = missingY;
Z = missingZ;
return(TrackingStateGait.NotTracked);
}
}
var jointPos = Joints[(int)joint];
X = jointPos.X;
Y = jointPos.Y;
Z = jointPos.Z;
return(jointPos.TrackingState);
}
/// <summary>
/// 單一關節Jog移動停止
/// </summary>
/// <param name="nRobotIndex">手臂代號</param>
/// <param name="nJoint">關節代號</param>
/// <returns>回傳是否成功,0:成功, 其他數字:失敗</returns>
public int JogJointStop(int nRobotIndex, Joints nJoint)
{
IRobotMotion.IRobotMotion IRobot = m_listRobots[nRobotIndex];
int nRet = IRobot.JogJointStop(nJoint);
return(nRet);
}
private void DataReceived(string res)
{
_responseChunks = res.Split(' ');
int resType = Convert.ToInt32(_responseChunks[0].Substring(1));
double[] data = new double[_responseChunks.Length - 1];
for (int i = 0; i < data.Length; i++)
{
// @TODO: add sanity like Double.TryParse(...)
data[i] = Double.Parse(_responseChunks[i + 1]);
}
switch (resType)
{
// ">21 400 300 500 0 0 1 0;"
case ABBCommunicationProtocol.RES_POSE:
this.initPos = new Vector(data[0], data[1], data[2]);
this.initRot = new Rotation(new Quaternion(data[3], data[4], data[5], data[6]));
break;
// ">22 0 0 0 0 90 0;"
case ABBCommunicationProtocol.RES_JOINTS:
this.initAx = new Joints(data[0], data[1], data[2], data[3], data[4], data[5]);
break;
}
}
/// <summary>
/// 單一關節移動(相對位置)(徑度)(無輸入速度)(不等待繼續)
/// </summary>
/// <param name="nRobotIndex">手臂代號</param>
/// <param name="nJoint">關節代號</param>
/// <param name="dbDegree">相對移動徑度值(相對位置)</param>
/// <returns>回傳是否成功,1:成功,-1:失敗</returns>
public int MoveJointRelRadian(int nRobotIndex, Joints nJoint, double dbRadian)
{
IRobotMotion.IRobotMotion IRobot = m_listRobots[nRobotIndex];
int nRet = IRobot.MoveJoint(nJoint, MovementsTypes.Rel, SpeedTypes.NoUse, WaitTypes.NoWait, dbRadian, 0, 0, 0);
return(1);
}
/// <summary>
/// 單一關節移動(相對位置)(徑度)(有輸入速度)(等待繼續)
/// </summary>
/// <param name="nRobotIndex">手臂代號</param>
/// <param name="nJoint">關節代號</param>
/// <param name="dbDegree">相對移動徑度值(相對位置)</param>
/// <param name="dbAcc">移動時加速度</param>
/// <param name="dbSpeed">移動時均速度</param>
/// <returns>回傳是否成功,1:成功,-1:失敗</returns>
public int MoveJointRelRadianSpeedWait(int nRobotIndex, Joints nJoint, double dbRadian, double dbAcc, double dbDec, double dbSpeed)
{
IRobotMotion.IRobotMotion IRobot = m_listRobots[nRobotIndex];
int nRet = IRobot.MoveJoint(nJoint, MovementsTypes.Rel, SpeedTypes.Use, WaitTypes.Wait, dbRadian, dbAcc, dbDec, dbSpeed);
return(1);
}
/// <summary>
/// 單一關節Jog移動
/// </summary>
/// <param name="nRobotIndex">手臂代號</param>
/// <param name="nJoint">關節代號</param>
/// <param name="nDir">移動方向</param>
/// <param name="dbSpeedPercentage">速度百分比</param>
/// <returns>回傳是否成功,0:成功, 其他數字:失敗</returns>
public int JogJoint(int nRobotIndex, Joints nJoint, Jog_Directions nDir, Jog_Speed nSpeed)
{
IRobotMotion.IRobotMotion IRobot = m_listRobots[nRobotIndex];
int nRet = IRobot.JogJoint(nJoint, nDir, nSpeed);
return(nRet);
/*
* IRobotMotion.IRobotMotion IRobot = m_listRobots[nRobotIndex];
* double dbAcc = 0;
* double dbDec = 0;
* double dbSpeed = 0;
* if (nSpeed == Jog_Speed.High)
* {
* dbAcc = 10000;dbDec = 10000; dbSpeed = 30;
* }
*
* else if (nSpeed == Jog_Speed.Mid)
* { dbAcc = 10000; dbDec = 10000; dbSpeed = 20; }
*
* else if (nSpeed == Jog_Speed.Mid)
* { dbAcc = 10000; dbDec = 10000; dbSpeed = 10; }
*
*
* double dbValue = 0;
* if (nDir == Jog_Directions.Plus)
* dbValue = 50;
* else if (nDir == Jog_Directions.Minus)
* dbValue = -50;
*
* int nRet = IRobot.MoveSingleJoint(nJoint, MovementsTypes.Abs, UnitTypes.Degree, SpeedTypes.Use, WaitTypes.NoWait, dbValue, dbAcc, dbDec, dbSpeed);
*/
return(nRet);
}
/// <summary>
/// 單一關節移動(絕對位置)(角度)(無輸入速度)(不等待繼續)
/// </summary>
/// <param name="nRobotIndex">手臂代號</param>
/// <param name="nJoint">關節代號</param>
/// <param name="dbDegree">絕對移動到角度值(絕對位置)</param>
/// <returns>回傳是否成功,1:成功,-1:失敗</returns>
public int JointAbsWait(int nRobotIndex, Joints nJoint, double dbDegree)
{
IRobotMotion.IRobotMotion IRobot = m_listRobots[nRobotIndex];
int nRet = IRobot.MoveJoint(nJoint, MovementsTypes.Abs, SpeedTypes.NoUse, WaitTypes.Wait, dbDegree, 0, 0, 0);
return(1);
}
/// <summary>
/// 單一關節移動(絕對位置)(角度)(輸入速度)(不等待繼續)
/// </summary>
/// <param name="nRobotIndex">手臂代號</param>
/// <param name="nJoint">關節代號</param>
/// <param name="dbDegree">絕對移動到角度值(絕對位置)</param>
/// <param name="dbAcc">加速度比例(0-100)</param>
/// <param name="dbDec">減速度比例(0-100)</param>
/// <param name="dbSpeed">最大速比例(0-100)</param>
/// <returns>回傳是否成功,1:成功,-1:失敗</returns>
public int MoveAbs(int nRobotIndex, Joints nJoint, double dbDegree, double dbAcc, double dbDec, double dbSpeed)
{
IRobotMotion.IRobotMotion IRobot = m_listRobots[nRobotIndex];
int nRet = IRobot.MoveJoint(nJoint, MovementsTypes.Abs, SpeedTypes.Use, WaitTypes.Wait, dbDegree, dbAcc, dbDec, dbSpeed);
return(1);
}
//for openni, we use an alternative version because the openni one suckso
public bool is_skeleton_tracked_alternative()
{
if (LastTrackedUser != null)
{
if (LastTrackedUser.SkeletonTracked == false || LastTrackedUser.PositionTracked == false)
{
return(false);
}
}
else
{
return(false);
}
//TODO test if its in current "crumpled" pose, needed for OpenNI
//instead we check neck and one arm)
if (Joints.ContainsKey(ZgJointId.LeftShoulder) &&
Joints.ContainsKey(ZgJointId.LeftElbow) &&
Joints.ContainsKey(ZgJointId.Neck) &&
Joints.ContainsKey(ZgJointId.Head))
{
if (get_relative_rotation(Joints[ZgJointId.LeftShoulder], Joints[ZgJointId.LeftElbow]).flat_rotation() == 0 &&
get_relative_rotation(Joints[ZgJointId.Neck], Joints[ZgJointId.Head]).flat_rotation() == 0)
{
return(false);
}
}
return(true);
}
/** Remove all joints from physics world.*/
public virtual void RemoveAllJoints(bool destroy = true)
{
foreach (var joint in Joints)
{
RemoveJointOrDelay(joint);
joint._world = null;
// clean the connection to this joint
if (destroy)
{
if (joint.BodyA != null)
{
joint.BodyA.RemoveJoint(joint);
}
if (joint.BodyB != null)
{
joint.BodyB.RemoveJoint(joint);
}
// test the distraction is delaied or not
if (DelayRemoveJoints.Exists(j => j == joint))
{
joint._destoryMark = true;
}
}
}
Joints.Clear();
}
private void LoadAnimations(Robot robot, Polymodel model)
{
robot.NumGuns = (sbyte)model.NumGuns;
for (int i = 0; i < 8; i++)
{
robot.GunPoints[i] = model.GunPoints[i];
robot.GunSubmodels[i] = (byte)model.GunSubmodels[i];
}
for (int m = 0; m < 9; m++)
{
for (int f = 0; f < 5; f++)
{
robot.AnimStates[m, f].NumJoints = 0;
robot.AnimStates[m, f].Offset = 0;
}
}
if (!model.IsAnimated)
{
return;
}
int[] gunNums = new int[10];
for (int i = 1; i < model.NumSubmodels; i++)
{
gunNums[i] = robot.NumGuns;
}
gunNums[0] = -1;
for (int g = 0; g < robot.NumGuns; g++)
{
int m = robot.GunSubmodels[g];
while (m != 0)
{
gunNums[m] = g;
m = model.Submodels[m].Parent;
}
}
for (int g = 0; g < robot.NumGuns + 1; g++)
{
for (int state = 0; state < 5; state++)
{
robot.AnimStates[g, state].NumJoints = 0;
robot.AnimStates[g, state].Offset = (short)(Joints.Count + VHAMFile.NumDescent2Joints);
for (int m = 0; m < model.NumSubmodels; m++)
{
if (gunNums[m] == g)
{
JointPos joint = new JointPos();
joint.JointNum = (short)m;
joint.Angles = model.AnimationMatrix[m, state];
Joints.Add(joint);
robot.AnimStates[g, state].NumJoints++;
}
}
}
}
}
[Fact] public void Range()
{
Data_Generator dg = new Data_Generator();
Hand_Generator hg = new Hand_Generator(dg);
int dat_len = dg.newInt(100);
List <Joints> dat_jL = hg.newJointsList(dat_len);
Joints exp_range = hg.newJoints();
Joints exp_min = hg.newJoints();
Joints exp_max = hg.newJoints();
Mock <IJointsHelper> mock_jh = new Mock <IJointsHelper>();
foreach (var j in dat_jL)
{
mock_jh.Setup(m => m.minMax(It.IsAny <Joints>(), It.IsAny <Joints>(), j)).Returns((exp_min, exp_max));
}
mock_jh.Setup(m => m.sub(exp_max, exp_min)).Returns(exp_range);
Stats s = new Stats(mock_jh.Object);
Joints act_range = s.range(dat_jL);
foreach (var j in dat_jL)
{
mock_jh.Verify(m => m.minMax(It.IsAny <Joints>(), It.IsAny <Joints>(), j), Times.Once());
}
mock_jh.Verify(m => m.sub(exp_max, exp_min), Times.Once());
test.jointsEqual(exp_range, act_range);
}
/// <summary>
///
/// </summary>
/// <param name="RobotPortNumber"></param>
/// <param name="Boudrate"></param>
/// <param name="JointID"></param>
public NineMansMorrisPlayer(DynamixelController controler, Joints jointIDs)
: base(controler, jointIDs)
{
//this.MyFigureShop = new FigureShop();
//this.CaptureShop = new FigureShop();
//this.BoardFigurePositions = new RobotBoardPosition();
}
public void ExportBMD(string fileName, bool isBDL)
{
string outDir = Path.GetDirectoryName(fileName);
string fileNameNoExt = Path.GetFileNameWithoutExtension(fileName);
fileNameNoExt = fileNameNoExt.Split('.')[0];
if (isBDL)
{
fileName = Path.Combine(outDir, fileNameNoExt + ".bdl");
}
else
{
fileName = Path.Combine(outDir, fileNameNoExt + ".bmd");
}
using (FileStream stream = new FileStream(fileName, FileMode.Create, FileAccess.Write))
{
EndianBinaryWriter writer = new EndianBinaryWriter(stream, Endian.Big);
if (isBDL)
{
writer.Write("J3D2bdl4".ToCharArray());
}
else
{
writer.Write("J3D2bmd3".ToCharArray());
}
writer.Write(0); // Placeholder for file size
if (isBDL)
{
writer.Write(9); // Number of sections; bmd has 8, bdl has 9
}
else
{
writer.Write(8);
}
writer.Write("SuperBMD - Gamma".ToCharArray());
Scenegraph.Write(writer, packetCount, vertexCount);
VertexData.Write(writer);
SkinningEnvelopes.Write(writer);
PartialWeightData.Write(writer);
Joints.Write(writer);
Shapes.Write(writer);
Materials.Write(writer);
if (isBDL)
{
MatDisplayList.Write(writer);
}
Textures.Write(writer);
writer.Seek(8, SeekOrigin.Begin);
writer.Write((int)writer.BaseStream.Length);
}
}
private void SetupJoints()
{
Joints.Clear();
Joints.Add(JointType.HipCenter, CreateJoint(JointType.HipCenter));
Joints.Add(JointType.HipLeft, CreateJoint(JointType.HipLeft));
Joints.Add(JointType.KneeLeft, CreateJoint(JointType.KneeLeft));
Joints.Add(JointType.AnkleLeft, CreateJoint(JointType.AnkleLeft));
Joints.Add(JointType.FootLeft, CreateJoint(JointType.FootLeft));
Joints.Add(JointType.HipRight, CreateJoint(JointType.HipRight));
Joints.Add(JointType.KneeRight, CreateJoint(JointType.KneeRight));
Joints.Add(JointType.AnkleRight, CreateJoint(JointType.AnkleRight));
Joints.Add(JointType.FootRight, CreateJoint(JointType.FootRight));
Joints.Add(JointType.Spine, CreateJoint(JointType.Spine));
Joints.Add(JointType.ShoulderCenter, CreateJoint(JointType.ShoulderCenter));
Joints.Add(JointType.ShoulderLeft, CreateJoint(JointType.ShoulderLeft));
Joints.Add(JointType.ElbowLeft, CreateJoint(JointType.ElbowLeft));
Joints.Add(JointType.WristLeft, CreateJoint(JointType.WristLeft));
Joints.Add(JointType.HandLeft, CreateJoint(JointType.HandLeft));
Joints.Add(JointType.ShoulderRight, CreateJoint(JointType.ShoulderRight));
Joints.Add(JointType.ElbowRight, CreateJoint(JointType.ElbowRight));
Joints.Add(JointType.WristRight, CreateJoint(JointType.WristRight));
Joints.Add(JointType.HandRight, CreateJoint(JointType.HandRight));
Joints.Add(JointType.Head, CreateJoint(JointType.Head));
}
public override string ToString()
{
StringBuilder builder = new StringBuilder();
builder.Append("<P,");
builder.Append(this.PoseId);
int numberOfJoints = Joints.Max(j => j.ServoId) + 1;
for (int i = 0; i < numberOfJoints; i++)
{
var joint = this.Joints.Where(j => j.ServoId == i).FirstOrDefault();
if (joint == null)
{
builder.Append(",-1,0");
}
else
{
builder.AppendFormat(",{0:F4},{1}", joint.Angle, joint.Duration);
}
}
builder.Append(">");
return(builder.ToString());
}
private TransformationMatrix ComputeWristFrame(TransformationMatrix currentTarget)
{
DenseMatrix addInverse = (DenseMatrix)Joints.Last().Value.JointTransformation.DenseMatrix *Joint6ToFlangeTrafo.DenseMatrix;
DenseMatrix wrist = currentTarget.DenseMatrix * (DenseMatrix)addInverse.Inverse();
return(new TransformationMatrix(wrist));
}
[Fact] public void HandToJoints()
{
Data_Generator dg = new Data_Generator();
Hand_Generator hg = new Hand_Generator(dg);
List <Vector[]> dat_vL = new List <Vector[]>()
{
dg.newVectors(5),
dg.newVectors(5),
dg.newVectors(5),
dg.newVectors(5),
dg.newVectors(5),
};
List <Finger> dat_fL = hg.newFingerList(dat_vL);
Hand dat_hand = hg.newHand(dat_fL);
Joints exp_joints = new Joints {
pinky = dat_vL[0],
ring = dat_vL[1],
middle = dat_vL[2],
index = dat_vL[3],
thumb = dat_vL[4],
palm = dat_hand.PalmPosition
};
VectorHelper vh = new VectorHelper();
JointsHelper jh = new JointsHelper(vh);
Joints act_joints = jh.handToJoints(dat_hand);
test.jointsEqual(exp_joints, act_joints);
}
public Pose(string pose) : this()
{
var poseParse = pose.Substring(0, pose.Length - 1).Split(",");
PoseId = Convert.ToInt32(poseParse[1]);
int i = 0;
for (int j = 2; j < poseParse.Length; j += 2)
{
var angle = Convert.ToDouble(poseParse[j]);
if (angle >= 0)
{
var joint = new Joint()
{
ServoId = i,
Angle = angle,
Duration = Convert.ToInt32((poseParse[j + 1]))
};
Joints.Add(joint);
}
i++;
}
}
//---------------------------------------------------------------------
//SAVING
//---------------------------------------------------------------------
public void Write(Stream stream)
{
//Brute force solution
//RenumberElements(HAMType.EClip);
//RenumberElements(HAMType.Weapon);
//RenumberElements(HAMType.Robot);
//RenumberElements(HAMType.Model);
//Science experiment
GenerateObjectBitmapTables(CompatObjBitmaps);
NumRobotJoints = 0;
Console.WriteLine("Loaded {0} joints", Joints.Count);
Joints.Clear();
foreach (Robot robot in Robots)
{
LoadAnimations(robot, Models[robot.ModelNum]);
}
Console.WriteLine("Constructed {0} joints", Joints.Count);
foreach (Reactor reactor in Reactors)
{
LoadReactorGuns(reactor);
}
LoadShipGuns(PlayerShip);
CreateDataLists();
BaseFile.Write(stream);
}
public void Export(string fileName)
{
using (FileStream stream = new FileStream(fileName, FileMode.Create, FileAccess.Write))
{
EndianBinaryWriter writer = new EndianBinaryWriter(stream, Endian.Big);
writer.Write("J3D2bmd3".ToCharArray());
writer.Write(0); // Placeholder for file size
writer.Write(8); // Number of sections; bmd has 8, bdl has 9
writer.Write("SVR3".ToCharArray());
writer.Write(-1);
writer.Write((long)-1);
Scenegraph.Write(writer, packetCount, vertexCount);
VertexData.Write(writer);
SkinningEnvelopes.Write(writer);
PartialWeightData.Write(writer);
Joints.Write(writer);
Shapes.Write(writer);
Materials.Write(writer);
Textures.Write(writer);
writer.Seek(8, SeekOrigin.Begin);
writer.Write((int)writer.BaseStream.Length);
}
}
public AmplitudeModulationControlPoint AquireTarget(Joints j) => new AmplitudeModulationControlPoint(
j.palm.x,
j.palm.z * -1,
j.palm.y,
GBL.UH_INTENSITY,
GBL.UH_FREQUENCY
);
public Vector3 this[Joints joint] {
get { return(_positions[(int)joint]); }
set {
_isSet[(int)joint] = true;
_positions[(int)joint] = value;
}
}
public void Dispose()
{
Springs.Dispose();
Joints.Dispose();
BlittableTransforms.Dispose();
Colliders.Dispose();
Logics.Dispose();
}
public void AddJoint(JointType joint, double radius, Brush brush)
{
Joints.Add(joint, new Ellipse
{
Width = radius,
Height = radius,
Fill = brush
});
}
public KinectBodyTracker(SmartKinectSensor smartSensor, ArduinoSerialPort arduinoSerialPort)
{
this.skeletonController = smartSensor.SkeletonController;
this.arduinoSerialPort = arduinoSerialPort;
this.joints = new Joints();
this.previousTimeStamp = DateTime.MinValue;
this.frameCount = 0;
CreateEmergencyStop();
LoadJointProfiles();
ConnectSkeletonController();
}
private void add_skeleton_joint( Joints joint, GameObject obj )
{
skeleton.Add (joint, obj);
inverse_skeleton.Add (obj, joint);
}
public Finger()
{
this.joints = new Joints();
}
public Vector3 get_raw_joint(Joints joint)
{
return raw_joint_pos[(int)joint];
}
