/// <summary> /// 操作点が球に接触(進入)していた際に発生する力を求める /// </summary> /// <param name="tipPosition">操作点の座標 [mm]</param> /// <param name="tipVelocity">操作点の速度 [mm/s]</param> /// <returns></returns> public Vector3D CalculateForce(Vector3D tipPosition, Vector3D tipVelocity) { Vector3D vec = tipPosition - this.Position; double distance = vec.Length; // 球の外や真ん中では力なし if (distance >= this.Radius || distance == 0) { return Vector3D.Zero; } vec /= distance; // 正規化 double f = this.Stiffness * (this.Radius - distance); if (f > this.ForceLimit) f = this.ForceLimit; return (f * vec) - this.Dumping * tipVelocity; }
/// <summary> /// 可動範囲を取得 /// </summary> private void LoadWorkspaceLimit() { double[] val = new double[6]; // 可動限界範囲を取得 Hd.hdGetDoublev(Hd.ParameterName.HD_MAX_WORKSPACE_DIMENSIONS, val); ErrorCheck("Getting max workspace"); WorkspaceMinimum = new Vector3D(val[0], val[1], val[2]); WorkspaceMaximum = new Vector3D(val[3], val[4], val[5]); // 推奨可動範囲を取得 Hd.hdGetDoublev(Hd.ParameterName.HD_USABLE_WORKSPACE_DIMENSIONS, val); ErrorCheck("Getting usable workspace"); UsableWorkspaceMinimum = new Vector3D(val[0], val[1], val[2]); UsableWorkspaceMaximum = new Vector3D(val[3], val[4], val[5]); // 机の高さを取得 float[] offset = new float[1]; Hd.hdGetFloatv(Hd.ParameterName.HD_TABLETOP_OFFSET, offset); ErrorCheck("Getting table-top offset"); TableTopOffset = (double)offset[0]; }
/// <summary> /// PHANTOM発揮力を設定します /// </summary> /// <param name="force">力のベクトル [N]</param> public void SetForce(Vector3D force) { double[] forceArray = force.ToArray(); Hd.hdSetDoublev(Hd.ParameterName.HD_CURRENT_FORCE, forceArray); }
/// <summary> /// 位置と半径を指定して球を作成 /// </summary> /// <param name="position">中心座標[mm]</param> /// <param name="radius">半径[mm]</param> public Orb(Vector3D position, double radius) { this.Position = position; this.Radius = radius; }
/// <summary> /// PHANTOMで呼ばれるメソッド。 /// ここで発揮力を設定するなどの処理を行う。 /// </summary> /// <returns>実行を繰り返したい場合はtrueを返す</returns> private bool ServoLoop() { // 現在のPHANTOM情報 HandPosition = Phantom.GetPosition(); // ジンバル部座標 [mm] HandVelocity = Phantom.GetVelocity(); // ジンバル部速度 [mm/s] // PHANTOMで発生させる力ベクトル [N] Vector3D force = Vector3D.Zero; // 剛体の球体を表現 foreach (ManagedPhantom.RigidPrimitives.Orb orb in RigidOrbs) { force += orb.CalculateForce(HandPosition); } // PHANTOMの発揮力を設定 Phantom.SetForce(force); // 終わる場合はfalse、繰り返す場合は true を返す return true; }