commit 157516d5095f4189d11c9f5d027d9c95a8609c06 Author: Shimazue Eiichi Date: Sat Nov 7 13:08:00 2026 +0900 [依頼107] デジタルツインで π0.5 の失敗の試行を再生し、置き場所の候補を確かめるコードを追加 Assets/TwinCheck/ の下に、実機の推論の記録の関節角を Unity のアームに直接置いて (ドライブ・物理は使わない)横から見る・箱との近さを測る・置き場所の候補の道すじを確かめる・ 上カメラを実機に合わせて並べる、Editor スクリプトと Python を置いた。 結果と README は KIOXIA の lab/twin/20261107_失敗の再生と置き場所/。 Co-Authored-By: Claude Opus 5.5 diff --git a/Assets/TwinCheck.meta b/Assets/TwinCheck.meta new file mode 100644 index 0000000..5f44e43 --- /dev/null +++ b/Assets/TwinCheck.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0bd33ee5bdb72451fab414dd9508c103 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor.meta b/Assets/TwinCheck/Editor.meta new file mode 100644 index 0000000..23d5cb1 --- /dev/null +++ b/Assets/TwinCheck/Editor.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 599790675b60e4368b067d6a2b8632e3 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinArm.cs b/Assets/TwinCheck/Editor/TwinArm.cs new file mode 100644 index 0000000..b057472 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinArm.cs @@ -0,0 +1,98 @@ +using UnityEngine; +using System.Collections.Generic; + +/// +/// TwinArm +/// 依頼107。シーンの写しの中の mechArm 1体に、関節角(度)を直接置く。 +/// ArticulationBody のドライブ(stiffness / damping / target)には一切触れず、 +/// 物理も回さない。親のアンカー → X軸まわりの回転 → 子のアンカーの逆、の順で +/// 各リンクの Transform を置く(ArticulationBody の回転関節と同じ式)。 +/// 爪先は FKScanner_MotionBased と同じく左右の tip Brep の中点、根元からの差で返す。 +/// +public class TwinArm +{ + public readonly GameObject root; + public readonly ArticulationBody baseBody; + public readonly ArticulationBody[] joints; // link1〜link6 + public readonly Transform leftTip, rightTip; + readonly Transform[] fingers = new Transform[6]; // left3, left2, left1, right3, right2, right1 + readonly Quaternion[] fingerInit = new Quaternion[6]; + public float sign = 1f; + + public TwinArm(GameObject go) + { + root = go; + var list = new List(); + foreach (var ab in go.GetComponentsInChildren(true)) + { + if (ab.isRoot) baseBody = ab; else list.Add(ab); + } + joints = list.GetRange(0, Mathf.Min(6, list.Count)).ToArray(); + leftTip = FindTip(go.transform, "left"); + rightTip = FindTip(go.transform, "right"); + string[] names = { "gripper_left3", "gripper_left2", "gripper_left1", "gripper_right3", "gripper_right2", "gripper_right1" }; + for (int i = 0; i < 6; i++) + { + fingers[i] = FindDeep(go.transform, names[i]); + if (fingers[i] != null) fingerInit[i] = fingers[i].localRotation; + } + } + + /// 関節角(度、j0〜j5)を置く。 + public void SetAngles(float[] deg) + { + for (int i = 0; i < joints.Length && i < deg.Length; i++) + { + var c = joints[i]; + var p = c.transform.parent; + Quaternion pr = p.rotation; + Vector3 jointPos = p.TransformPoint(c.parentAnchorPosition); + Quaternion jointRot = pr * c.parentAnchorRotation * Quaternion.AngleAxis(sign * deg[i], Vector3.right); + Quaternion childRot = jointRot * Quaternion.Inverse(c.anchorRotation); + Vector3 childPos = jointPos - childRot * Vector3.Scale(c.transform.lossyScale, c.anchorPosition); + c.transform.SetPositionAndRotation(childPos, childRot); + } + } + + /// 爪の開き。0 = 開き(GripperController の初期)、40 = 閉じ(GripperController.Close)。 + public void SetGrip(float a) + { + float[] s = { a, a, -a, -a, -a, a }; + for (int i = 0; i < 6; i++) + if (fingers[i] != null) fingers[i].localRotation = fingerInit[i] * Quaternion.Euler(0, s[i], 0); + } + + /// 爪先(左右 tip Brep の中点)の、根元からの位置(m、Unity 座標)。 + public Vector3 TipLocal() + { + Vector3 mid = (leftTip.position + rightTip.position) * 0.5f; + return mid - baseBody.transform.position; + } + + /// 腕の描画だけを切り替える(GameObject は消さない)。 + public void SetVisible(bool on) + { + foreach (var r in root.GetComponentsInChildren(true)) r.enabled = on; + } + + public Vector3 TipWorld() => (leftTip.position + rightTip.position) * 0.5f; + + static Transform FindTip(Transform root, string side) + { + var g = FindDeep(root, $"gripper_{side}1"); + if (g == null) return null; + var i = FindDeep(g, "Instance-Def-210103_4302_101"); + return i == null ? null : i.Find("Brep"); + } + + public static Transform FindDeep(Transform parent, string name) + { + foreach (Transform child in parent) + { + if (child.name == name) return child; + var r = FindDeep(child, name); + if (r != null) return r; + } + return null; + } +} diff --git a/Assets/TwinCheck/Editor/TwinArm.cs.meta b/Assets/TwinCheck/Editor/TwinArm.cs.meta new file mode 100644 index 0000000..97b4daf --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinArm.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 3f2c3f128c0034837ab674dc5ea05b33 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinCheckA.cs b/Assets/TwinCheck/Editor/TwinCheckA.cs new file mode 100644 index 0000000..0b4d62d --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckA.cs @@ -0,0 +1,69 @@ +using UnityEngine; +using UnityEditor; +using UnityEditor.SceneManagement; +using System.Collections.Generic; +using System.IO; +using System.Text; +using System.Globalization; + +/// +/// TwinCheckA +/// 依頼107 の A。FK テーブルから選んだ行の関節角を、シーンの写しの20体それぞれに置き、 +/// 爪先の位置(根元からの差)を書き出す。表の ee との比較は Python 側で行う。 +/// シーンは開くだけで保存しない。 +/// 実行: Unity -batchmode -quit -executeMethod TwinCheckA.Run -twinIn {行の CSV} -twinOut {出力 CSV} [-twinSign 1|-1] +/// +public static class TwinCheckA +{ + public const string ScenePath = "Assets/TwinCheck/TwinCheck_Base.unity"; + + public static void Run() + { + string inPath = TwinCheckArgs.Get("-twinIn", ""); + string outPath = TwinCheckArgs.Get("-twinOut", ""); + float sign = float.Parse(TwinCheckArgs.Get("-twinSign", "1"), CultureInfo.InvariantCulture); + + EditorSceneManager.OpenScene(ScenePath, OpenSceneMode.Single); + var arms = FindArms(); + + var lines = File.ReadAllLines(inPath); + var sb = new StringBuilder(); + sb.AppendLine("row_index,robot,tip_x,tip_y,tip_z,arm_min_y,near_box_mm,near_block_mm"); + for (int li = 1; li < lines.Length; li++) + { + var c = lines[li].Split(','); + if (c.Length < 10) continue; + var deg = new float[6]; + for (int k = 0; k < 6; k++) deg[k] = float.Parse(c[4 + k], CultureInfo.InvariantCulture); + for (int r = 0; r < arms.Count; r++) + { + arms[r].sign = sign; + arms[r].SetAngles(deg); + var t = arms[r].TipLocal(); + string extra = ""; + if (r == 0) + { + // 1体目だけ: 腕(link1〜)の見た目の一番低い所と、箱・ブロックへの近さ(コライダの外形どうしの距離) + var box = GameObject.Find("WhiteBox_arm1"); + var block = GameObject.Find("RedBlock_arm1"); + extra = string.Format(CultureInfo.InvariantCulture, ",{0:F6},{1:F1},{2:F1}", + TwinGeom.ArmMinY(arms[r]), TwinGeom.ArmToObjectMm(arms[r], box), TwinGeom.ArmToObjectMm(arms[r], block)); + } + sb.AppendLine(string.Format(CultureInfo.InvariantCulture, "{0},{1},{2:F6},{3:F6},{4:F6}", c[0], arms[r].root.name, t.x, t.y, t.z) + extra); + } + } + File.WriteAllText(outPath, sb.ToString()); + Debug.Log($"[TwinCheckA] wrote {outPath} arms={arms.Count}"); + } + + public static List FindArms() + { + var arms = new List(); + for (int r = 1; r <= 20; r++) + { + var go = GameObject.Find($"mecharm270mpi_{r}_0"); + if (go != null) arms.Add(new TwinArm(go)); + } + return arms; + } +} diff --git a/Assets/TwinCheck/Editor/TwinCheckA.cs.meta b/Assets/TwinCheck/Editor/TwinCheckA.cs.meta new file mode 100644 index 0000000..04dc29e --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckA.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 722a2ff69071f4426ad2c32f595edfd3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinCheckB.cs b/Assets/TwinCheck/Editor/TwinCheckB.cs new file mode 100644 index 0000000..46ac92e --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckB.cs @@ -0,0 +1,177 @@ +using UnityEngine; +using System.Collections.Generic; +using System.IO; +using System.Text; + +/// +/// TwinCheckB +/// 依頼107 の B。実機の推論の記録(実際に届いた関節角)を Unity のアームに置いて測り、撮る。 +/// 角度は TwinArm.SetAngles で直接置く(ドライブの値は変えない・物理は回さない)。 +/// +/// Metrics: trials.csv(ブロック・箱の位置)と timeline.csv(時刻ごとの j0〜j5・爪)から、 +/// 時刻ごとの爪先の位置・ブロックとの水平のずれ・箱の壁との近さを書く。 +/// 実行: Unity -batchmode -quit -executeMethod TwinCheckB.Metrics -twinTrials {trials.csv} -twinTimeline {timeline.csv} -twinOut {出力 CSV} +/// Render: shots.csv の1行ごとに姿勢・物の位置・カメラを決めて PNG を撮る。 +/// 実行: Unity -batchmode -quit -executeMethod TwinCheckB.Render -twinShots {shots.csv} +/// +public static class TwinCheckB +{ + public static void Metrics() + { + var trials = TwinCsv.Read(TwinCheckArgs.Get("-twinTrials", "")); + var timeline = TwinCsv.Read(TwinCheckArgs.Get("-twinTimeline", "")); + string outPath = TwinCheckArgs.Get("-twinOut", ""); + var sc = TwinScene.Open(); + + var byRun = new Dictionary>>(); + foreach (var r in timeline) + { + if (!byRun.TryGetValue(r["run"], out var l)) byRun[r["run"]] = l = new List>(); + l.Add(r); + } + + var sb = new StringBuilder(); + sb.AppendLine("run,i,t,grip,stage,tip_x,tip_y,tip_z,tip_h_mm,tip_block_xz_mm,d_block_grip_mm," + + "d_box_tip_mm,d_box_gripper_mm,d_box_forearm_mm,in_box_tip,in_box_gripper,in_box_forearm,in_box_depth_mm"); + var sw = System.Diagnostics.Stopwatch.StartNew(); + int n = 0; + foreach (var tr in trials) + { + if (!byRun.TryGetValue(tr["run"], out var rows)) continue; + sc.Place(TwinScene.F(tr["block_x_m"]), TwinScene.F(tr["block_z_m"]), Yaw(tr, "block_yaw_deg"), + TwinScene.F(tr["box_x_m"]), TwinScene.F(tr["box_z_m"]), Yaw(tr, "box_yaw_deg")); + Vector3 bc = sc.block.transform.position; + Vector3 xc = sc.box.transform.position; + foreach (var r in rows) + { + var deg = new float[6]; + for (int k = 0; k < 6; k++) deg[k] = TwinScene.F(r["j" + k]); + int grip = int.Parse(r["grip"]); + sc.arm.SetAngles(deg); + sc.arm.SetGrip(grip == 1 ? 40f : 0f); + Vector3 tip = sc.arm.TipWorld(); + float hBlock = new Vector2(tip.x - bc.x, tip.z - bc.z).magnitude * 1000f; + + // 箱の近くにいるときだけ、部位ごとの距離を細かく測る(遠いときは粗い値で足りる) + float far = new Vector2(tip.x - xc.x, tip.z - xc.z).magnitude; + string boxCols; + if (far < 0.22f) + { + float dT = TwinGeom.NearestMm(sc.arm, sc.box, TwinGeom.Part.Tip, out _); + float dG = TwinGeom.NearestMm(sc.arm, sc.box, TwinGeom.Part.Gripper, out _); + float dF = TwinGeom.NearestMm(sc.arm, sc.box, TwinGeom.Part.Forearm, out _); + int iT = TwinGeom.InsideBox(sc.arm, sc.box, TwinGeom.Part.Tip, out float d1); + int iG = TwinGeom.InsideBox(sc.arm, sc.box, TwinGeom.Part.Gripper, out float d2); + int iF = TwinGeom.InsideBox(sc.arm, sc.box, TwinGeom.Part.Forearm, out float d3); + boxCols = $"{dT:F1},{dG:F1},{dF:F1},{iT},{iG},{iF},{Mathf.Max(d1, Mathf.Max(d2, d3)):F1}"; + } + else boxCols = ",,,,,,"; + float dBlk = hBlock < 120f + ? Mathf.Min(TwinGeom.NearestMm(sc.arm, sc.block, TwinGeom.Part.Tip, out _), TwinGeom.NearestMm(sc.arm, sc.block, TwinGeom.Part.Gripper, out _)) + : float.NaN; + sb.AppendLine($"{r["run"]},{r["i"]},{r["t"]},{grip},{r["stage"]},{TwinScene.S(tip.x)},{TwinScene.S(tip.y)},{TwinScene.S(tip.z)}," + + $"{tip.y * 1000f:F1},{hBlock:F1},{(float.IsNaN(dBlk) ? "" : dBlk.ToString("F1"))},{boxCols}"); + n++; + } + } + File.WriteAllText(outPath, sb.ToString()); + Debug.Log($"[TwinCheckB] metrics rows={n} sec={sw.Elapsed.TotalSeconds:F1} -> {outPath}"); + } + + static float Yaw(Dictionary tr, string key) => + tr.TryGetValue(key, out var v) && !string.IsNullOrEmpty(v) ? TwinScene.F(v) : 0f; + + /// + /// shots.csv の列: out,j0..j5,grip,block_x,block_z,block_yaw,box_x,box_z,box_yaw,view,target_x,target_z,w,h + /// view = side(基準点を通る、根元からの向きに直角な水平の視線・正射影 0.24m 高)/ front(根元からの向きの先から根元を見る) + /// / top(真上からの仮の上カメラ)/ cam(cam_* 列のカメラ位置・向き・画角をそのまま使う) + /// + // 横から見た画の範囲(正射影)。twin_overlay.py の目盛りもこの値を使う + public const float SideCenterY = 0.10f; + public const float SideHalfHeight = 0.15f; + + public static void Render() + { + var shots = TwinCsv.Read(TwinCheckArgs.Get("-twinShots", "")); + var sc = TwinScene.Open(); + var camGo = new GameObject("TwinCheckCam"); + var cam = camGo.AddComponent(); + cam.clearFlags = CameraClearFlags.SolidColor; + cam.backgroundColor = new Color(0.82f, 0.84f, 0.88f); + cam.nearClipPlane = 0.01f; + cam.farClipPlane = 10f; + var main = GameObject.Find("Main Camera"); + if (main) main.SetActive(false); + + // B の確かめ用の補助の光(view=cam / top のときは消して、シーンの素の光だけで撮る) + var fill = new GameObject("TwinCheckFill").AddComponent(); + fill.type = LightType.Directional; + fill.intensity = 0.6f; + fill.shadows = LightShadows.None; + + foreach (var s in shots) + { + // 腕を隠すときは描画だけを切る(SetActive で消して戻すと、ArticulationBody がリンクの位置を戻してしまうため) + sc.arm.SetVisible(s.TryGetValue("hide_arm", out var ha) ? ha != "1" : true); + var deg = new float[6]; + for (int k = 0; k < 6; k++) deg[k] = TwinScene.F(s["j" + k]); + sc.arm.SetAngles(deg); + sc.arm.SetGrip(s["grip"] == "1" ? 40f : 0f); + sc.Place(TwinScene.F(s["block_x"]), TwinScene.F(s["block_z"]), TwinScene.F(s["block_yaw"]), + TwinScene.F(s["box_x"]), TwinScene.F(s["box_z"]), TwinScene.F(s["box_yaw"])); + sc.block.SetActive(s.TryGetValue("hide_block", out var hb) ? hb != "1" : true); + int w = int.Parse(s["w"]), h = int.Parse(s["h"]); + var tgt = new Vector3(TwinScene.F(s["target_x"]), 0f, TwinScene.F(s["target_z"])); + Vector3 dir = new Vector3(tgt.x, 0, tgt.z).normalized; + if (dir.sqrMagnitude < 1e-6f) dir = Vector3.forward; + string view = s["view"]; + fill.enabled = view == "side" || view == "front"; + cam.ResetProjectionMatrix(); + cam.aspect = (float)w / h; + if (view == "side") + { + Vector3 perp = new Vector3(dir.z, 0, -dir.x); // 根元からの向きに直角 + if (perp.z < 0) perp = -perp; // 手前(+Z 側)から見る + cam.orthographic = true; + cam.orthographicSize = SideHalfHeight; + cam.transform.position = tgt + perp * 0.8f + Vector3.up * SideCenterY; + cam.transform.rotation = Quaternion.LookRotation(-perp, Vector3.up); + fill.transform.rotation = Quaternion.LookRotation(-perp + Vector3.down * 0.3f, Vector3.up); + } + else if (view == "front") + { + cam.orthographic = false; + cam.fieldOfView = 40f; + cam.transform.position = tgt + dir * 0.45f + Vector3.up * 0.12f; + cam.transform.LookAt(new Vector3(tgt.x * 0.5f, 0.06f, tgt.z * 0.5f)); + fill.transform.rotation = cam.transform.rotation; + } + else if (view == "top") + { + cam.orthographic = false; + cam.fieldOfView = float.Parse(s.TryGetValue("cam_fov", out var fv) && fv != "" ? fv : "60", System.Globalization.CultureInfo.InvariantCulture); + cam.transform.position = new Vector3(0f, TwinScene.F(s.TryGetValue("cam_h", out var ch) && ch != "" ? ch : "0.75"), 0.136f); + cam.transform.rotation = Quaternion.LookRotation(Vector3.down, Vector3.back); + } + else // cam + { + cam.orthographic = false; + cam.transform.position = new Vector3(TwinScene.F(s["cam_x"]), TwinScene.F(s["cam_y"]), TwinScene.F(s["cam_z"])); + cam.transform.rotation = new Quaternion(TwinScene.F(s["cam_qx"]), TwinScene.F(s["cam_qy"]), TwinScene.F(s["cam_qz"]), TwinScene.F(s["cam_qw"])); + cam.fieldOfView = TwinScene.F(s["cam_fov"]); + if (s.TryGetValue("cam_cx", out var cx) && cx != "") + { + // 画像の中心のずれ(主点)も合わせるときは、投影行列を作り直す + float fy = 0.5f * h / Mathf.Tan(cam.fieldOfView * 0.5f * Mathf.Deg2Rad); + float cxp = TwinScene.F(cx), cyp = TwinScene.F(s["cam_cy"]); + float n = cam.nearClipPlane, f = cam.farClipPlane; + float l = -cxp * n / fy, r = (w - cxp) * n / fy; + float t = cyp * n / fy, b = -(h - cyp) * n / fy; + cam.projectionMatrix = Matrix4x4.Frustum(l, r, b, t, n, f); + } + } + TwinScene.Shoot(cam, w, h, s["out"]); + } + Debug.Log($"[TwinCheckB] rendered {shots.Count}"); + } +} diff --git a/Assets/TwinCheck/Editor/TwinCheckB.cs.meta b/Assets/TwinCheck/Editor/TwinCheckB.cs.meta new file mode 100644 index 0000000..a5ef4b3 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckB.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: fd0a190ce8e4e47b29a1af06f47477cd +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinCheckC.cs b/Assets/TwinCheck/Editor/TwinCheckC.cs new file mode 100644 index 0000000..b0c8ee1 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckC.cs @@ -0,0 +1,118 @@ +using UnityEngine; +using System.Collections.Generic; +using System.Globalization; +using System.IO; +using System.Text; + +/// +/// TwinCheckC +/// 依頼107 の C。置き場所の候補ごとに、HOME → ブロックの真上 → つかむ(開)→ つかむ(閉)→ 持ち上げ → 白箱の上 +/// を関節角の直線補間(区間ごとに 40 刻み)で動かし、箱の壁・床・ブロックとの近さを測る。 +/// 角度は TwinArm.SetAngles で直接置く(ドライブの値は変えない・物理は回さない)。運んでいるブロックは描かない。 +/// 実行: Unity -batchmode -quit -executeMethod TwinCheckC.Path -twinPoses {c_poses.csv} -twinOut {c_path.csv} -twinShots {c_shots.csv} -twinCam {camera_fit.json の値を並べた CSV} +/// +public static class TwinCheckC +{ + static readonly float[] Home = { 0, 0, 0, 0, 0, -30 }; + const int Steps = 40; + + public static void Path() + { + var poses = TwinCsv.Read(TwinCheckArgs.Get("-twinPoses", "")); + string outPath = TwinCheckArgs.Get("-twinOut", ""); + string shotsPath = TwinCheckArgs.Get("-twinShots", ""); + var cam = TwinCsv.Read(TwinCheckArgs.Get("-twinCam", ""))[0]; + string imgDir = TwinCheckArgs.Get("-twinImgDir", ""); + var sc = TwinScene.Open(); + + var byPoint = new Dictionary>>(); + var order = new List(); + foreach (var p in poses) + { + if (!byPoint.ContainsKey(p["point"])) { byPoint[p["point"]] = new Dictionary>(); order.Add(p["point"]); } + byPoint[p["point"]][p["phase"]] = p; + } + + var sb = new StringBuilder("point,complete,box_min_mm,box_min_part,box_min_segment,box_inside,box_inside_depth_mm,floor_min_mm,floor_min_segment," + + "block_min_before_grasp_mm,block_min_part\n"); + var shots = new StringBuilder("out,j0,j1,j2,j3,j4,j5,grip,block_x,block_z,block_yaw,box_x,box_z,box_yaw,view,target_x,target_z,w,h,hide_arm,hide_block," + + "cam_x,cam_y,cam_z,cam_qx,cam_qy,cam_qz,cam_qw,cam_fov\n"); + string camCols = $"{cam["cam_x"]},{cam["cam_y"]},{cam["cam_z"]},{cam["cam_qx"]},{cam["cam_qy"]},{cam["cam_qz"]},{cam["cam_qw"]},{cam["cam_fov"]}"; + + foreach (var pt in order) + { + var ph = byPoint[pt]; + if (!(ph.ContainsKey("above") && ph.ContainsKey("grasp") && ph.ContainsKey("lift") && ph.ContainsKey("box"))) + { + sb.AppendLine($"{pt},no,,,,,,,,,"); + continue; + } + var any = ph["grasp"]; + float bx = TwinScene.F(any["px"]), bz = TwinScene.F(any["pz"]); + float xx = TwinScene.F(any["box_x"]), xz = TwinScene.F(any["box_z"]); + sc.Place(bx, bz, 0f, xx, xz, 0f); + + // 区間: (名前, 始め, 終わり, 爪) + var segs = new List<(string, float[], float[], float)> + { + ("home_to_above", Home, J(ph["above"]), 0f), + ("above_to_grasp", J(ph["above"]), J(ph["grasp"]), 0f), + ("grasp_to_lift", J(ph["grasp"]), J(ph["lift"]), 40f), + ("lift_to_box", J(ph["lift"]), J(ph["box"]), 40f), + }; + float boxMin = float.MaxValue, floorMin = float.MaxValue, blkMin = float.MaxValue, depth = 0f; + string boxPart = "", boxSeg = "", floorSeg = "", blkPart = ""; + bool inside = false; + float[] boxAt = null; float boxGrip = 0f; + foreach (var (name, a, b, grip) in segs) + { + for (int s = 0; s <= Steps; s++) + { + float t = (float)s / Steps; + var q = new float[6]; + for (int k = 0; k < 6; k++) q[k] = Mathf.Lerp(a[k], b[k], t); + sc.arm.SetAngles(q); + sc.arm.SetGrip(grip); + foreach (var part in new[] { TwinGeom.Part.Tip, TwinGeom.Part.Gripper, TwinGeom.Part.Forearm }) + { + float d = TwinGeom.NearestMm(sc.arm, sc.box, part, out _); + if (d < boxMin) { boxMin = d; boxPart = part.ToString(); boxSeg = $"{name}@{s}"; boxAt = q; boxGrip = grip; } + if (TwinGeom.InsideBox(sc.arm, sc.box, part, out float dep) > 0) { inside = true; depth = Mathf.Max(depth, dep); } + // つかむ前にブロックへ当たらないか: HOME→真上は全部位、真上→つかむは前腕だけ + //(爪・爪先はブロックを挟みに行くので、近いのは当たり前) + if (name == "home_to_above" || (name == "above_to_grasp" && part == TwinGeom.Part.Forearm)) + { + float db = TwinGeom.NearestMm(sc.arm, sc.block, part, out _); + if (db < blkMin) { blkMin = db; blkPart = part.ToString(); } + } + } + float fy = TwinGeom.ArmMinY(sc.arm) * 1000f; + if (fy < floorMin) { floorMin = fy; floorSeg = $"{name}@{s}"; } + } + } + sb.AppendLine($"{pt},yes,{boxMin:F1},{boxPart},{boxSeg},{(inside ? "yes" : "no")},{depth:F1},{floorMin:F1},{floorSeg},{blkMin:F1},{blkPart}"); + + string common = $"{TwinScene.S(bx)},{TwinScene.S(bz)},0,{TwinScene.S(xx)},{TwinScene.S(xz)},0"; + var g = J(ph["grasp"]); + string gj = string.Join(",", System.Array.ConvertAll(g, v => v.ToString("F3", CultureInfo.InvariantCulture))); + // 上カメラ(D で合わせたもの)から、つかむ直前(爪は開)。腕あり・腕なし(隠れの割合を数えるため) + shots.AppendLine($"{imgDir}/{pt}_top_grasp.png,{gj},0,{common},cam,0,0,960,540,0,0,{camCols}"); + shots.AppendLine($"{imgDir}/{pt}_top_grasp_noarm.png,{gj},0,{common},cam,0,0,960,540,1,0,{camCols}"); + // 箱にいちばん近づいた刻みを横から + string bj = string.Join(",", System.Array.ConvertAll(boxAt, v => v.ToString("F3", CultureInfo.InvariantCulture))); + shots.AppendLine($"{imgDir}/{pt}_side_boxmin.png,{bj},{(boxGrip > 0 ? 1 : 0)},{common},side,{TwinScene.S(xx)},{TwinScene.S(xz)},960,720,0,1,{camCols}"); + // つかむ姿勢を横から + shots.AppendLine($"{imgDir}/{pt}_side_grasp.png,{gj},0,{common},side,{TwinScene.S(bx)},{TwinScene.S(bz)},960,720,0,0,{camCols}"); + } + File.WriteAllText(outPath, sb.ToString()); + File.WriteAllText(shotsPath, shots.ToString()); + Debug.Log($"[TwinCheckC] points={order.Count} -> {outPath}"); + } + + static float[] J(Dictionary r) + { + var q = new float[6]; + for (int k = 0; k < 6; k++) q[k] = TwinScene.F(r["j" + k]); + return q; + } +} diff --git a/Assets/TwinCheck/Editor/TwinCheckC.cs.meta b/Assets/TwinCheck/Editor/TwinCheckC.cs.meta new file mode 100644 index 0000000..253c25f --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckC.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9d97b95bd28fd40eeb1f51379cfc06cf +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinCheckD.cs b/Assets/TwinCheck/Editor/TwinCheckD.cs new file mode 100644 index 0000000..5ddf683 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckD.cs @@ -0,0 +1,48 @@ +using UnityEngine; +using System.IO; +using System.Text; + +/// +/// TwinCheckD +/// 依頼107 の D。上カメラ合わせの手がかりにする、HOME([0,0,0,0,0,-30])のときの爪の点の 3D 位置を書き出す。 +/// 左右の爪先(tip Brep の中心=見た目の外形の中心)と、爪の付け根の上面の中心。 +/// 実行: Unity -batchmode -quit -executeMethod TwinCheckD.Points -twinOut {出力 json} +/// +public static class TwinCheckD +{ + public static void Points() + { + string outPath = TwinCheckArgs.Get("-twinOut", ""); + var sc = TwinScene.Open(); + sc.arm.SetAngles(new float[] { 0, 0, 0, 0, 0, -30 }); + sc.arm.SetGrip(0f); + var sb = new StringBuilder("{\n"); + void Add(string name, Vector3 p, bool last = false) => + sb.Append($" \"{name}\": [{p.x:F5}, {p.y:F5}, {p.z:F5}]{(last ? "" : ",")}\n"); + Add("left_tip_origin", sc.arm.leftTip.position); + Add("right_tip_origin", sc.arm.rightTip.position); + Add("left_tip_bounds_center", Bounds(sc.arm.leftTip).center); + Add("right_tip_bounds_center", Bounds(sc.arm.rightTip).center); + Add("left_tip_bounds_top", Top(Bounds(sc.arm.leftTip))); + Add("right_tip_bounds_top", Top(Bounds(sc.arm.rightTip))); + var gb = TwinArm.FindDeep(sc.arm.root.transform, "gripper_base"); + Add("gripper_base_bounds_top", Top(Bounds(gb))); + Add("gripper_base_bounds_center", Bounds(gb).center); + var b = Bounds(sc.arm.joints[5].transform); + Add("link6_bounds_min", b.min); + Add("link6_bounds_max", b.max); + Add("tip_mid", sc.arm.TipWorld(), true); + sb.Append("}\n"); + File.WriteAllText(outPath, sb.ToString()); + } + + static Bounds Bounds(Transform t) + { + var rs = t.GetComponentsInChildren(true); + var b = rs.Length > 0 ? rs[0].bounds : new Bounds(t.position, Vector3.zero); + foreach (var r in rs) b.Encapsulate(r.bounds); + return b; + } + + static Vector3 Top(Bounds b) => new Vector3(b.center.x, b.max.y, b.center.z); +} diff --git a/Assets/TwinCheck/Editor/TwinCheckD.cs.meta b/Assets/TwinCheck/Editor/TwinCheckD.cs.meta new file mode 100644 index 0000000..97b706d --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckD.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f85723c5a0f434882b8d2448a3735603 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinCheckDump.cs b/Assets/TwinCheck/Editor/TwinCheckDump.cs new file mode 100644 index 0000000..67b540f --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckDump.cs @@ -0,0 +1,100 @@ +using UnityEngine; +using UnityEditor; +using UnityEditor.SceneManagement; +using System.IO; +using System.Text; + +/// +/// TwinCheckDump +/// 依頼107 の下調べ。プレハブ mecharm270mpi_0_0 の関節(ArticulationBody)の並びと +/// アンカー・ドライブの値を書き出すだけ。シーン・プレハブは保存しない。 +/// 実行: Unity -batchmode -executeMethod TwinCheckDump.Run -twinOut {出力ファイル} +/// +public static class TwinCheckDump +{ + const string PrefabPath = "Assets/Prefabs/mecharm270mpi_0_0.prefab"; + + public static void Run() + { + string outPath = TwinCheckArgs.Get("-twinOut", "/tmp/twincheck/dump.txt"); + var sb = new StringBuilder(); + + EditorSceneManager.NewScene(NewSceneSetup.EmptyScene, NewSceneMode.Single); + var prefab = AssetDatabase.LoadAssetAtPath(PrefabPath); + var go = (GameObject)PrefabUtility.InstantiatePrefab(prefab); + go.transform.position = Vector3.zero; + + sb.AppendLine("== hierarchy =="); + DumpTree(go.transform, 0, sb); + + sb.AppendLine("== articulation bodies =="); + foreach (var ab in go.GetComponentsInChildren(true)) + { + var d = ab.xDrive; + sb.AppendLine($"{Path(ab.transform)} root={ab.isRoot} type={ab.jointType} " + + $"anchorPos={V(ab.anchorPosition)} anchorRot={Q(ab.anchorRotation)} " + + $"parentAnchorPos={V(ab.parentAnchorPosition)} parentAnchorRot={Q(ab.parentAnchorRotation)} " + + $"stiff={d.stiffness} damp={d.damping} target={d.target} lo={d.lowerLimit} hi={d.upperLimit} " + + $"twistLock={ab.twistLock} immovable={ab.immovable} gravity={ab.useGravity} " + + $"localPos={V(ab.transform.localPosition)} localRot={Q(ab.transform.localRotation)} enabled={ab.enabled}"); + } + + File.WriteAllText(outPath, sb.ToString()); + Debug.Log($"[TwinCheckDump] wrote {outPath}"); + } + + /// シーンの写しの、ロボット以外の物(床・ブロック・箱・カメラ・光)の位置と大きさを書き出す。 + public static void RunScene() + { + string outPath = TwinCheckArgs.Get("-twinOut", "/tmp/twincheck/scene.txt"); + var sb = new StringBuilder(); + EditorSceneManager.OpenScene(TwinCheckA.ScenePath, OpenSceneMode.Single); + foreach (var go in UnityEngine.SceneManagement.SceneManager.GetActiveScene().GetRootGameObjects()) + { + var t = go.transform; + var comps = new StringBuilder(); + foreach (var c in go.GetComponents()) if (c != null && !(c is Transform)) comps.Append(c.GetType().Name).Append(' '); + var rend = go.GetComponentInChildren(); + string b = rend != null ? $" bounds c={V(rend.bounds.center)} s={V(rend.bounds.size)} mat={(rend.sharedMaterial ? rend.sharedMaterial.name : "-")}" : ""; + sb.AppendLine($"{go.name} active={go.activeSelf} pos={V(t.position)} rot={t.eulerAngles} scale={V(t.lossyScale)} [{comps}]{b}"); + if (!go.name.StartsWith("mecharm")) + foreach (Transform ch in t) + { + var r2 = ch.GetComponent(); + sb.AppendLine($" {ch.name} pos={V(ch.position)} scale={V(ch.lossyScale)}" + (r2 ? $" bounds c={V(r2.bounds.center)} s={V(r2.bounds.size)}" : "")); + } + var cam = go.GetComponent(); + if (cam) sb.AppendLine($" camera fov={cam.fieldOfView} near={cam.nearClipPlane} far={cam.farClipPlane} ortho={cam.orthographic}"); + } + foreach (var ab in GameObject.Find("mecharm270mpi_1_0").GetComponentsInChildren(true)) + { + var d = ab.xDrive; + sb.AppendLine($"AB {ab.name} type={ab.jointType} lo={d.lowerLimit} hi={d.upperLimit} motion={ab.twistLock} swingY={ab.swingYLock} swingZ={ab.swingZLock} target={d.target} stiff={d.stiffness} damp={d.damping} limit={d.forceLimit} " + + $"parentAnchorPos={V(ab.parentAnchorPosition)} parentAnchorRot={Q(ab.parentAnchorRotation)} anchorRot={Q(ab.anchorRotation)} matchAnchors={ab.matchAnchors} collide={ab.GetComponentsInChildren(true).Length}"); + } + File.WriteAllText(outPath, sb.ToString()); + } + + static void DumpTree(Transform t, int depth, StringBuilder sb) + { + var comps = t.GetComponents(); + var names = new StringBuilder(); + foreach (var c in comps) if (c != null && !(c is Transform)) names.Append(c.GetType().Name).Append(' '); + sb.AppendLine($"{new string(' ', depth * 2)}{t.name} pos={V(t.position)} [{names}]"); + foreach (Transform c in t) DumpTree(c, depth + 1, sb); + } + + static string Path(Transform t) => t.parent == null ? t.name : Path(t.parent) + "/" + t.name; + static string V(Vector3 v) => $"({v.x:F5},{v.y:F5},{v.z:F5})"; + static string Q(Quaternion q) => $"({q.x:F5},{q.y:F5},{q.z:F5},{q.w:F5})"; +} + +public static class TwinCheckArgs +{ + public static string Get(string name, string def) + { + var a = System.Environment.GetCommandLineArgs(); + for (int i = 0; i < a.Length - 1; i++) if (a[i] == name) return a[i + 1]; + return def; + } +} diff --git a/Assets/TwinCheck/Editor/TwinCheckDump.cs.meta b/Assets/TwinCheck/Editor/TwinCheckDump.cs.meta new file mode 100644 index 0000000..8ecf220 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCheckDump.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 28d0f8a41a41742629330f8cbbba43c2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinCsv.cs b/Assets/TwinCheck/Editor/TwinCsv.cs new file mode 100644 index 0000000..c4a049e --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCsv.cs @@ -0,0 +1,46 @@ +using System.Collections.Generic; +using System.IO; +using System.Text; + +/// 依頼107。見出し付き CSV を読む(ダブルクォートの中のカンマに対応)。 +public static class TwinCsv +{ + public static List> Read(string path) + { + var rows = new List>(); + var lines = File.ReadAllLines(path, Encoding.UTF8); + if (lines.Length == 0) return rows; + var head = Split(lines[0]); + for (int i = 1; i < lines.Length; i++) + { + if (string.IsNullOrWhiteSpace(lines[i])) continue; + var c = Split(lines[i]); + var d = new Dictionary(); + for (int k = 0; k < head.Count && k < c.Count; k++) d[head[k]] = c[k]; + rows.Add(d); + } + return rows; + } + + static List Split(string line) + { + var o = new List(); + var sb = new StringBuilder(); + bool q = false; + for (int i = 0; i < line.Length; i++) + { + char ch = line[i]; + if (q) + { + if (ch == '"' && i + 1 < line.Length && line[i + 1] == '"') { sb.Append('"'); i++; } + else if (ch == '"') q = false; + else sb.Append(ch); + } + else if (ch == '"') q = true; + else if (ch == ',') { o.Add(sb.ToString()); sb.Clear(); } + else sb.Append(ch); + } + o.Add(sb.ToString()); + return o; + } +} diff --git a/Assets/TwinCheck/Editor/TwinCsv.cs.meta b/Assets/TwinCheck/Editor/TwinCsv.cs.meta new file mode 100644 index 0000000..0471298 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinCsv.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d222e40e04e384729b8811bc20f576e8 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinGeom.cs b/Assets/TwinCheck/Editor/TwinGeom.cs new file mode 100644 index 0000000..14ed66a --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinGeom.cs @@ -0,0 +1,142 @@ +using UnityEngine; +using System.Collections.Generic; + +/// +/// TwinGeom +/// 依頼107。腕の見た目のメッシュ(頂点を間引いたもの)と、箱の壁・ブロックの直方体との +/// 距離を測る。物理は使わない。直方体は Transform の位置・回転・lossyScale(単位立方体)とみなす。 +/// +public static class TwinGeom +{ + public enum Part { Tip, Gripper, Forearm, Other } + + class Cached { public Renderer r; public Vector3[] v; public Part part; } + static readonly Dictionary> cache = new Dictionary>(); + + static List Get(TwinArm arm) + { + if (cache.TryGetValue(arm.root, out var list)) return list; + list = new List(); + foreach (var r in arm.joints[0].GetComponentsInChildren(true)) + { + var mf = r.GetComponent(); + if (mf == null || mf.sharedMesh == null) continue; + var all = mf.sharedMesh.vertices; + int stride = Mathf.Max(1, all.Length / 1500); + var v = new List(); + for (int i = 0; i < all.Length; i += stride) v.Add(all[i]); + list.Add(new Cached { r = r, v = v.ToArray(), part = Classify(arm, r.transform) }); + } + cache[arm.root] = list; + return list; + } + + static Part Classify(TwinArm arm, Transform t) + { + if (t == arm.leftTip || t == arm.rightTip || IsUnder(t, arm.leftTip?.parent) || IsUnder(t, arm.rightTip?.parent)) return Part.Tip; + for (var p = t; p != null; p = p.parent) + { + if (p.name.StartsWith("gripper")) return Part.Gripper; + if (p == arm.joints[2].transform || p == arm.joints[3].transform || p == arm.joints[4].transform || p == arm.joints[5].transform) return Part.Forearm; + if (p == arm.root.transform) break; + } + return Part.Other; + } + + static bool IsUnder(Transform t, Transform parent) + { + if (parent == null) return false; + for (var p = t; p != null; p = p.parent) if (p == parent) return true; + return false; + } + + public static float ArmMinY(TwinArm arm) + { + float m = float.MaxValue; + foreach (var c in Get(arm)) + { + var tr = c.r.transform; + foreach (var v in c.v) m = Mathf.Min(m, tr.TransformPoint(v).y); + } + return m - arm.baseBody.transform.position.y; + } + + /// 直方体(単位立方体を Transform で置いたもの)と点の距離(m)。中なら負(めり込みの深さ)。 + public static float BoxDistance(Transform box, Vector3 p) + { + Vector3 half = box.lossyScale * 0.5f; + Vector3 q = Quaternion.Inverse(box.rotation) * (p - box.position); + Vector3 d = new Vector3(Mathf.Abs(q.x) - half.x, Mathf.Abs(q.y) - half.y, Mathf.Abs(q.z) - half.z); + Vector3 o = Vector3.Max(d, Vector3.zero); + float outside = o.magnitude; + float inside = Mathf.Min(Mathf.Max(d.x, Mathf.Max(d.y, d.z)), 0f); + return outside + inside; + } + + static IEnumerable Solids(GameObject obj) + { + if (obj.GetComponent() != null) yield return obj.transform; + // 箱の「縁」を見るため、底板(Bottom)は距離の相手に入れない + foreach (Transform ch in obj.transform) if (ch.GetComponent() != null && ch.name != "Bottom") yield return ch; + } + + /// 腕(link1 から先)と物の、いちばん近い距離(mm)。 + public static float ArmToObjectMm(TwinArm arm, GameObject obj) => NearestMm(arm, obj, null, out _); + + /// 部位ごとの、物へのいちばん近い距離(mm)。part を null にすると全部。 + public static float NearestMm(TwinArm arm, GameObject obj, Part? part, out Vector3 nearestPoint) + { + float m = float.MaxValue; + nearestPoint = Vector3.zero; + if (obj == null) return m; + var solids = new List(Solids(obj)); + foreach (var c in Get(arm)) + { + if (part.HasValue && c.part != part.Value) continue; + var tr = c.r.transform; + foreach (var v in c.v) + { + var p = tr.TransformPoint(v); + foreach (var s in solids) + { + float d = BoxDistance(s, p); + if (d < m) { m = d; nearestPoint = p; } + } + } + } + return m * 1000f; + } + + /// + /// 箱の内側(壁の内面より内・底より上・壁の上端より下)に入っている頂点の数と、 + /// 入り込んだ深さ(壁の上端から下へ、mm)の最大。箱は Bottom/Front/Back/Left/Right の5枚でできている前提。 + /// + public static int InsideBox(TwinArm arm, GameObject box, Part? part, out float maxDepthMm) + { + maxDepthMm = 0f; + var f = box.transform.Find("Front"); var b = box.transform.Find("Back"); + var l = box.transform.Find("Left"); var r = box.transform.Find("Right"); + if (f == null || b == null || l == null || r == null) return -1; + float xMin = Mathf.Min(l.position.x, r.position.x) + l.lossyScale.x * 0.5f; + float xMax = Mathf.Max(l.position.x, r.position.x) - r.lossyScale.x * 0.5f; + float zMin = Mathf.Min(f.position.z, b.position.z) + f.lossyScale.z * 0.5f; + float zMax = Mathf.Max(f.position.z, b.position.z) - b.lossyScale.z * 0.5f; + float top = f.position.y + f.lossyScale.y * 0.5f; + int n = 0; + foreach (var c in Get(arm)) + { + if (part.HasValue && c.part != part.Value) continue; + var tr = c.r.transform; + foreach (var v in c.v) + { + var p = tr.TransformPoint(v); + if (p.x > xMin && p.x < xMax && p.z > zMin && p.z < zMax && p.y < top) + { + n++; + maxDepthMm = Mathf.Max(maxDepthMm, (top - p.y) * 1000f); + } + } + } + return n; + } +} diff --git a/Assets/TwinCheck/Editor/TwinGeom.cs.meta b/Assets/TwinCheck/Editor/TwinGeom.cs.meta new file mode 100644 index 0000000..722ce63 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinGeom.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4946c894ed6444426bba68e21b54b7e3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Editor/TwinScene.cs b/Assets/TwinCheck/Editor/TwinScene.cs new file mode 100644 index 0000000..564a578 --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinScene.cs @@ -0,0 +1,72 @@ +using UnityEngine; +using UnityEditor.SceneManagement; +using System.Globalization; +using System.IO; + +/// +/// TwinScene +/// 依頼107。シーンの写し(Assets/TwinCheck/TwinCheck_Base.unity)を開き、 +/// 1体目のアーム(mecharm270mpi_1_0、根元が原点)・赤ブロック・白箱・床・光だけを残して見せる。 +/// 他の物は SetActive(false) にするだけで、シーンは保存しない。 +/// +public class TwinScene +{ + public TwinArm arm; + public GameObject block, box, floor; + + public static TwinScene Open() + { + EditorSceneManager.OpenScene(TwinCheckA.ScenePath, OpenSceneMode.Single); + var s = new TwinScene(); + var keep = new[] { "mecharm270mpi_1_0", "RedBlock_arm1", "WhiteBox_arm1", "Plane1", "Directional Light", "Main Camera" }; + foreach (var go in UnityEngine.SceneManagement.SceneManager.GetActiveScene().GetRootGameObjects()) + go.SetActive(System.Array.IndexOf(keep, go.name) >= 0); + s.arm = new TwinArm(GameObject.Find("mecharm270mpi_1_0")); + s.block = GameObject.Find("RedBlock_arm1"); + s.box = GameObject.Find("WhiteBox_arm1"); + s.floor = GameObject.Find("Plane1"); + // 物理で動かないように(物理は回さないが念のため) + var rb = s.block.GetComponent(); + if (rb) rb.isKinematic = true; + return s; + } + + /// ブロック(24mm 角、マットの上)と白箱(80mm 角・高さ 43mm は仮)を置く。yaw は画像から求めた向き(度)。 + public void Place(float blockX, float blockZ, float blockYaw, float boxX, float boxZ, float boxYaw) + { + block.transform.SetPositionAndRotation(new Vector3(blockX, 0.012f, blockZ), Quaternion.Euler(0, -blockYaw, 0)); + box.transform.SetPositionAndRotation(new Vector3(boxX, 0f, boxZ), Quaternion.Euler(0, -Wrap45(boxYaw), 0)); + } + + public static float Wrap45(float yaw) + { + yaw %= 90f; + if (yaw > 45f) yaw -= 90f; + if (yaw < -45f) yaw += 90f; + return yaw; + } + + /// カメラから RenderTexture に撮って PNG に書く。 + public static void Shoot(Camera cam, int w, int h, string path) + { + var rt = new RenderTexture(w, h, 24, RenderTextureFormat.ARGB32) { antiAliasing = 4 }; + var prev = cam.targetTexture; + cam.targetTexture = rt; + cam.Render(); + var act = RenderTexture.active; + RenderTexture.active = rt; + var tex = new Texture2D(w, h, TextureFormat.RGB24, false); + tex.ReadPixels(new Rect(0, 0, w, h), 0, 0); + tex.Apply(); + RenderTexture.active = act; + cam.targetTexture = prev; + Directory.CreateDirectory(Path.GetDirectoryName(path)); + File.WriteAllBytes(path, tex.EncodeToPNG()); + Object.DestroyImmediate(tex); + rt.Release(); + Object.DestroyImmediate(rt); + } + + public static float F(string s) => float.Parse(s, CultureInfo.InvariantCulture); + public static string S(float v, string fmt = "F4") => v.ToString(fmt, CultureInfo.InvariantCulture); +} diff --git a/Assets/TwinCheck/Editor/TwinScene.cs.meta b/Assets/TwinCheck/Editor/TwinScene.cs.meta new file mode 100644 index 0000000..c9f93fd --- /dev/null +++ b/Assets/TwinCheck/Editor/TwinScene.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6b85b5c066bb54764ac32b5cc08c37b2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/TwinCheck/Python~/twin_a.py b/Assets/TwinCheck/Python~/twin_a.py new file mode 100644 index 0000000..e6ab934 --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_a.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +"""依頼107 A: FK テーブルの行の関節角を Unity に置いた爪先(TwinCheckA の出力)と、表の ee のずれをまとめる。 + + python3 twin_a.py {行の CSV} {TwinCheckA の出力} {まとめの CSV} +腕(link1 から先)がマットの下に入る・箱・ブロックに触れる姿勢は「接触あり」として分けて数える +(表は物理を 0.5 秒回してから測っているので、接触すると角度どおりの位置で止まらない)。 +""" +import csv +import math +import statistics as st +import sys + + +def main(rows_csv, tip_csv, out_csv): + ref = {r["row_index"]: r for r in csv.DictReader(open(rows_csv))} + out = [] + per_robot = {} + for r in csv.DictReader(open(tip_csv)): + e = ref[r["row_index"]] + d = math.dist([float(r["tip_x"]), float(r["tip_y"]), float(r["tip_z"])], + [float(e["ee_x"]), float(e["ee_y"]), float(e["ee_z"])]) * 1000 + per_robot.setdefault(r["robot"], []).append(d) + if r["robot"] != "mecharm270mpi_1_0": + continue + contact = float(r["arm_min_y"]) < 0 or float(r["near_box_mm"]) < 0 or float(r["near_block_mm"]) < 0 + out.append(dict(row_index=r["row_index"], **{k: e[k] for k in ("ee_x", "ee_y", "ee_z", "j0", "j1", "j2", "j3", "j4", "j5")}, + unity_x=r["tip_x"], unity_y=r["tip_y"], unity_z=r["tip_z"], diff_mm=round(d, 2), + arm_min_y_mm=round(float(r["arm_min_y"]) * 1000, 1), near_box_mm=r["near_box_mm"], + near_block_mm=r["near_block_mm"], contact="yes" if contact else "no")) + with open(out_csv, "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=list(out[0].keys())) + w.writeheader() + w.writerows(out) + ds = [o["diff_mm"] for o in out] + free = [o["diff_mm"] for o in out if o["contact"] == "no"] + print(f"行 {len(ds)}: 最大 {max(ds):.1f}mm 中央値 {st.median(ds):.1f}mm / 接触なし {len(free)} 行: 最大 {max(free):.1f}mm 中央値 {st.median(free):.1f}mm " + f"/ 10mm 超 {sum(d > 10 for d in ds)} 行(うち接触あり {sum(o['diff_mm'] > 10 and o['contact'] == 'yes' for o in out)})") + spread = max(abs(max(v) - max(per_robot['mecharm270mpi_1_0'])) for v in per_robot.values()) + print(f"20体のあいだの差(最大のずれの差): {spread:.2f}mm") + + +if __name__ == "__main__": + main(*sys.argv[1:4]) diff --git a/Assets/TwinCheck/Python~/twin_c.py b/Assets/TwinCheck/Python~/twin_c.py new file mode 100644 index 0000000..3158c1f --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_c.py @@ -0,0 +1,246 @@ +#!/usr/bin/env python3 +"""依頼107 C: 次に実機で試す置き場所の候補を、FK テーブルと Unity で確かめる。 + + select : 各点で、肩 j1 <= 88° の行から 4 つの姿勢(ブロックの真上・つかむ・持ち上げ・白箱の上)の候補を + 上位 K 個ずつ選び、C/c_candidates.csv に書く(Unity で角度を置き直して表の ee と比べるため) + finalize : Unity で置き直した爪先(C/c_candidates_fk.csv)が表の ee と 5mm 以内の候補のうち、 + いちばん順位の高いものを採り、C/c_poses.csv(Unity の道すじの確かめ・撮影の入力)を書く + table : Unity の結果(C/c_path.csv・C/images/*)から、1点1行の表 C/c_table.csv と各点の画像を作る + +姿勢の選び方(どれも肩 j1 <= 88°) + つかむ : 爪先の高さ 10〜40mm(Codex の条件)。水平のずれ + 0.2×|高さ−20mm| が小さい順 + (20mm は B の成功5件の閉じた瞬間の爪先の高さ 20〜25mm に合わせた目安) + 真上 : 高さ 60〜100mm。水平のずれ 15mm 以内のうち、つかむ姿勢との関節の差(j0〜j4 の絶対値の和)が小さい順 + 持ち上げ : 高さ 80〜130mm。同じく、つかむ姿勢との関節の差が小さい順 + 白箱の上 : 白箱の中心(B の検出の中央値)の上、高さ 80〜130mm。水平のずれ 15mm 以内のうち、持ち上げ姿勢との関節の差が小さい順 +""" +import csv +import json +import math +import sys +from pathlib import Path + +import numpy as np + +OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所") +FK = Path("/Volumes/KIOXIA/r2r2r-data/fk_tables/fk_scan_motion_based_v2.csv") +J1_MAX = 88.0 +K = 8 +GRID_X = [120, 160, 200] +GRID_Z = [180, 220, 260] +HOME = [0, 0, 0, 0, 0, -30] +FK_OK_MM = 5.0 + + +def box_center(): + xs, zs = [], [] + for r in csv.DictReader(open(OUT / "B/trials.csv")): + if r["box_pos_source"] == "detected": + xs.append(float(r["box_x_m"])) + zs.append(float(r["box_z_m"])) + return float(np.median(xs)), float(np.median(zs)) + + +def points(): + pts = [dict(point=f"G_x{x}_z{z}", kind="grid", x=x / 1000, z=z / 1000) for x in GRID_X for z in GRID_Z] + for r in csv.DictReader(open(OUT / "C/teach_pick_fk.csv")): + if r["robot"] != "mecharm270mpi_1_0": + continue + pts.append(dict(point=f"T{r['row_index']}", kind="teach", x=float(r["tip_x"]), z=float(r["tip_z"]))) + return pts + + +def load_fk(): + a = np.loadtxt(FK, delimiter=",", skiprows=1) + a = a[a[:, 4] <= J1_MAX] + return a # ee_x, ee_y, ee_z, j0..j5 + + +def select(): + a = load_fk() + bx, bz = box_center() + rows = [] + for p in points(): + dxz = np.hypot(a[:, 0] - p["x"], a[:, 2] - p["z"]) + y = a[:, 1] + m = (y >= 0.010) & (y <= 0.040) + score = dxz * 1000 + 0.2 * np.abs(y - 0.020) * 1000 + idx = np.where(m)[0] + g = idx[np.argsort(score[idx])][:K] + for rank, i in enumerate(g): + rows.append((p, "grasp", rank, i)) + gj = a[g[0], 3:8] + for phase, lo, hi, ref, cx, cz in (("above", 0.060, 0.100, gj, p["x"], p["z"]), + ("lift", 0.080, 0.130, gj, p["x"], p["z"])): + d2 = np.hypot(a[:, 0] - cx, a[:, 2] - cz) + mm = (a[:, 1] >= lo) & (a[:, 1] <= hi) & (d2 <= 0.015) + idx = np.where(mm)[0] + if len(idx) == 0: + mm = (a[:, 1] >= lo) & (a[:, 1] <= hi) + idx = np.where(mm)[0] + order = idx[np.argsort(d2[idx])][:K] + else: + jd = np.abs(a[idx, 3:8] - ref).sum(axis=1) + order = idx[np.argsort(jd)][:K] + for rank, i in enumerate(order): + rows.append((p, phase, rank, i)) + if phase == "lift": + lj = a[order[0], 3:8] + d2 = np.hypot(a[:, 0] - bx, a[:, 2] - bz) + mm = (a[:, 1] >= 0.080) & (a[:, 1] <= 0.130) & (d2 <= 0.015) + idx = np.where(mm)[0] + if len(idx): + jd = np.abs(a[idx, 3:8] - lj).sum(axis=1) + order = idx[np.argsort(jd)][:K] + for rank, i in enumerate(order): + rows.append((p, "box", rank, i)) + with open(OUT / "C/c_candidates.csv", "w") as f: + f.write("row_index,ee_x,ee_y,ee_z,j0,j1,j2,j3,j4,j5,point,phase,rank,target_x,target_z\n") + for k, (p, phase, rank, i) in enumerate(rows): + r = a[i] + tx, tz = (bx, bz) if phase == "box" else (p["x"], p["z"]) + f.write(f"{k}," + ",".join(f"{v:.6f}" for v in r[:3]) + "," + ",".join(f"{v:.3f}" for v in r[3:9]) + + f",{p['point']},{phase},{rank},{tx:.4f},{tz:.4f}\n") + print("候補", len(rows), "白箱の中心", bx, bz) + + +def finalize(): + cand = {r["row_index"]: r for r in csv.DictReader(open(OUT / "C/c_candidates.csv"))} + fk = {r["row_index"]: r for r in csv.DictReader(open(OUT / "C/c_candidates_fk.csv")) if r["robot"] == "mecharm270mpi_1_0"} + best = {} + for k, c in cand.items(): + f = fk[k] + d = math.dist([float(c["ee_x"]), float(c["ee_y"]), float(c["ee_z"])], + [float(f["tip_x"]), float(f["tip_y"]), float(f["tip_z"])]) * 1000 + c["table_vs_fk_mm"] = d + c.update(fk_x=f["tip_x"], fk_y=f["tip_y"], fk_z=f["tip_z"], arm_min_y=f["arm_min_y"]) + key = (c["point"], c["phase"]) + if d <= FK_OK_MM and (key not in best or int(c["rank"]) < int(best[key]["rank"])): + best[key] = c + bx, bz = box_center() + pts = points() + out = [] + for p in pts: + for phase in ("above", "grasp", "lift", "box"): + c = best.get((p["point"], phase)) + if c is None: + continue + out.append(dict(point=p["point"], kind=p["kind"], px=p["x"], pz=p["z"], phase=phase, rank=c["rank"], + fk_row=c["row_index"], **{f"j{i}": c[f"j{i}"] for i in range(6)}, + ee_x=c["ee_x"], ee_y=c["ee_y"], ee_z=c["ee_z"], fk_x=c["fk_x"], fk_y=c["fk_y"], fk_z=c["fk_z"], + table_vs_fk_mm=f"{c['table_vs_fk_mm']:.1f}", + target_x=c["target_x"], target_z=c["target_z"], box_x=f"{bx:.4f}", box_z=f"{bz:.4f}")) + with open(OUT / "C/c_poses.csv", "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=list(out[0].keys())) + w.writeheader() + w.writerows(out) + miss = [(p["point"], ph) for p in pts for ph in ("above", "grasp", "lift", "box") if (p["point"], ph) not in best] + print("採った姿勢", len(out), "選べなかった", miss) + + +VIS_OK = 0.5 # 上から見えるブロックの割合がこれ以上なら「見える」 +BOX_HIT_MM = -10.0 # 箱の壁にこれより深くめり込めば「当たる」 +NEAR_MM = 10.0 # 箱の壁・床から ±10mm 以内は「すれすれ」(数 mm の差で決めない) +GRASP_XZ_OK_MM = 10.0 + + +def red_pixels(path): + import cv2 + im = cv2.imread(str(path)).astype(int) + b, g, r = im[..., 0], im[..., 1], im[..., 2] + return int(((r > 60) & (r > 2 * g) & (r > 2 * b)).sum()) + + +def table(): + import cv2 + from PIL import Image, ImageDraw, ImageFont + font = ImageFont.truetype("/System/Library/Fonts/ヒラギノ角ゴシック W3.ttc", 24) + small = ImageFont.truetype("/System/Library/Fonts/ヒラギノ角ゴシック W3.ttc", 19) + poses = {} + for r in csv.DictReader(open(OUT / "C/c_poses.csv")): + poses.setdefault(r["point"], {})[r["phase"]] = r + path = {r["point"]: r for r in csv.DictReader(open(OUT / "C/c_path.csv"))} + rows = [] + for p in points(): + pt = p["point"] + ph = poses.get(pt, {}) + pa = path.get(pt, {}) + row = dict(point=pt, kind="教えた位置" if p["kind"] == "teach" else "格子の候補", + target_x_mm=round(p["x"] * 1000, 1), target_z_mm=round(p["z"] * 1000, 1)) + reasons = [] + if pa.get("complete") != "yes": + row.update(usable="使えない", reason="表の中で、つかむ・真上・持ち上げの姿勢を選べなかった(候補の行は、角度を置き直すと腕が床の下に入る=表を作ったときに床に当たって止まった値)") + rows.append(row) + continue + for phase in ("above", "grasp", "lift", "box"): + r = ph[phase] + row[f"{phase}_angles"] = "[" + ", ".join(f"{float(r[f'j{i}']):.1f}" for i in range(6)) + "]" + g = ph["grasp"] + row["shoulder_j1_grasp_deg"] = float(g["j1"]) + row["shoulder_j1_max_deg"] = max(float(ph[k]["j1"]) for k in ph) + gx, gy, gz = float(g["fk_x"]), float(g["fk_y"]), float(g["fk_z"]) + row["grasp_tip_xz_err_mm"] = round(math.hypot(gx - p["x"], gz - p["z"]) * 1000, 1) + row["grasp_tip_h_mm"] = round(gy * 1000, 1) + row["table_vs_unity_max_mm"] = max(float(ph[k]["table_vs_fk_mm"]) for k in ph) + bxz = [float(ph["box"]["fk_x"]), float(ph["box"]["fk_z"])] + row["above_box_xz_err_mm"] = round(math.hypot(bxz[0] - float(ph["box"]["box_x"]), bxz[1] - float(ph["box"]["box_z"])) * 1000, 1) + row["above_box_tip_h_mm"] = round(float(ph["box"]["fk_y"]) * 1000, 1) + bm = float(pa["box_min_mm"]) + row.update(box_min_mm=bm, box_min_part=pa["box_min_part"], box_min_where=pa["box_min_segment"], + box_hit="当たる" if bm < BOX_HIT_MM else ("すれすれ" if bm <= NEAR_MM else "当たらない"), + floor_min_mm=float(pa["floor_min_mm"]), floor_min_where=pa["floor_min_segment"], + block_min_before_grasp_mm=float(pa["block_min_before_grasp_mm"])) + n_arm = red_pixels(OUT / f"C/images/{pt}_top_grasp.png") + n_free = red_pixels(OUT / f"C/images/{pt}_top_grasp_noarm.png") + vis = n_arm / n_free if n_free else 0.0 + row["top_visible_ratio"] = round(vis, 2) + row["top_visible"] = "見える" if vis >= VIS_OK else "隠れる" + if row["grasp_tip_xz_err_mm"] > GRASP_XZ_OK_MM: + reasons.append(f"つかむ姿勢の爪先が狙いから {row['grasp_tip_xz_err_mm']}mm 離れる") + if row["box_hit"] == "当たる": + reasons.append(f"{row['box_min_part']}が箱に当たる") + elif row["box_hit"] == "すれすれ": + reasons.append(f"{row['box_min_part']}が箱の縁すれすれ({bm:+.0f}mm)") + if row["floor_min_mm"] < -NEAR_MM: + reasons.append(f"腕が床の下 {-row['floor_min_mm']:.0f}mm に入る") + elif row["floor_min_mm"] < 0: + # つかむ姿勢で爪先がマットの近く(0〜10mm)に来るのは当たり前なので、床の下に入るときだけ書く + reasons.append(f"腕が床の下に少し入る({row['floor_min_mm']:+.1f}mm、{row['floor_min_where']}。±{NEAR_MM:.0f}mm 以内なので当たるとは決めない)") + if vis < VIS_OK: + reasons.append(f"上から見てブロックが {100 * (1 - vis):.0f}% 隠れる") + hard = [x for x in reasons if "当たる" in x and "決めない" not in x or "mm に入る" in x or "隠れる" in x or "離れる" in x] + row["usable"] = "使えない" if hard else ("要注意" if reasons else "使えそう") + row["reason"] = " / ".join(reasons) if reasons else "どの条件にも当たらない" + rows.append(row) + + # 画像(1点1枚): 上カメラから見たつかむ直前 / 横から見たつかむ姿勢 / 箱にいちばん近づいたとき + top = cv2.imread(str(OUT / f"C/images/{pt}_top_grasp.png")) + s1 = cv2.resize(cv2.imread(str(OUT / f"C/images/{pt}_side_grasp.png")), (720, 540)) + s2 = cv2.resize(cv2.imread(str(OUT / f"C/images/{pt}_side_boxmin.png")), (720, 540)) + canvas = cv2.cvtColor(np.hstack([top, s1, s2]), cv2.COLOR_BGR2RGB) + im = Image.fromarray(canvas) + d = ImageDraw.Draw(im) + d.rectangle([0, 0, 960, 68], fill=(255, 255, 255)) + d.rectangle([0, 502, 960, 540], fill=(255, 255, 255)) + d.text((10, 8), f"{pt}({row['kind']}・x={row['target_x_mm']} z={row['target_z_mm']}mm) 判定: {row['usable']}", font=font, fill=(0, 0, 0)) + d.text((10, 40), f"上カメラ(D で合わせたもの)から、つかむ直前。ブロックが見える割合 {vis:.0%}", font=small, fill=(0, 0, 0)) + d.text((970, 8), f"横から、つかむ姿勢(肩 j1={row['shoulder_j1_grasp_deg']:.1f}°・爪先の高さ {row['grasp_tip_h_mm']}mm)", font=small, fill=(0, 0, 0)) + d.text((1690, 8), f"横から、箱にいちばん近いとき({row['box_min_part']} {bm:.0f}mm)", font=small, fill=(0, 0, 0)) + d.text((10, 510), row["reason"][:70], font=small, fill=(150, 0, 0)) + im.save(OUT / f"C/c_{pt}.png") + + cols = ["point", "kind", "target_x_mm", "target_z_mm", "usable", "reason", + "above_angles", "grasp_angles", "lift_angles", "box_angles", "shoulder_j1_grasp_deg", "shoulder_j1_max_deg", + "grasp_tip_xz_err_mm", "grasp_tip_h_mm", "table_vs_unity_max_mm", "above_box_xz_err_mm", "above_box_tip_h_mm", + "box_min_mm", "box_min_part", "box_min_where", "box_hit", "floor_min_mm", "floor_min_where", + "block_min_before_grasp_mm", "top_visible_ratio", "top_visible"] + with open(OUT / "C/c_table.csv", "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=cols, extrasaction="ignore") + w.writeheader() + w.writerows(rows) + for r in rows: + print(r["point"], r["usable"], r.get("top_visible_ratio"), r.get("grasp_tip_xz_err_mm"), r.get("shoulder_j1_max_deg"), r.get("box_min_mm"), r.get("floor_min_mm"), r["reason"]) + + +if __name__ == "__main__": + {"select": select, "finalize": finalize, "table": table}[sys.argv[1]]() diff --git a/Assets/TwinCheck/Python~/twin_camera_d.py b/Assets/TwinCheck/Python~/twin_camera_d.py new file mode 100644 index 0000000..af2cd47 --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_camera_d.py @@ -0,0 +1,176 @@ +#!/usr/bin/env python3 +"""依頼107 D: Unity のカメラを実機の上カメラに合わせる。 + +合わせ方 + 1. 実機の1コマ(アームが HOME の最初のコマ)でマットの四隅を検出する(本線の mat_corner_detection.detect_corners) + 2. 画素は正方形・画像の中心が光軸・レンズのゆがみなしと仮定する + 3. マットの上の2隅は画像の上の端で切れていて本当の角ではない(v=0)。そこで、 + - 下の2隅(本当の角)は (±310, 361) mm の点として合わせる + - 上の2点は「マットの左右の縁 x=±310mm の上のどこか」(奥行き z は未知)として合わせる + - HOME の爪先の青い印2つ(実機の画像)を、Unity の HOME の左右の爪先(tip Brep の位置)に合わせる + 焦点距離・カメラの位置3・向き3・上の2点の奥行き2 の 9 個を、10 個の式の最小二乗で解く(scipy)。 + ホモグラフィだけ・四隅だけでは焦点距離が決まらない(ほぼ真下向きのため。試すと 2 万 px 台の外れた値になる) + 4. 残ったずれ: 各点の再投影の誤差(px)。爪先の印は Unity と実機のアームの姿勢のずれも含む +出力: {OUT}/D/camera_fit.json(Unity のカメラの位置・回転・画角) +""" +import json +import math +import sys +from pathlib import Path + +import cv2 +import numpy as np + +OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所") +RUNS = Path("/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy") +sys.path.insert(0, str(Path.home() / "r2r2r-scripts")) +MAT_CORNERS_MM = [[310, -89], [-310, -89], [-310, 361], [310, 361]] +RUN = "pi05_20261101_191547" # 補正なしの失敗。最初のコマはアームが HOME、ブロックと箱が見える + + +def focal_from_h(G, cx, cy): + """G: マットの点(X,Z,1) → 画素 の 3x3。K=[f,0,cx;0,f,cy;0,0,1] として列の直交と長さの等しさから f を求める。""" + T = np.array([[1, 0, -cx], [0, 1, -cy], [0, 0, 1]], float) + A = T @ G # = diag(f,f,1) [r1 r2 t] + h1, h2 = A[:, 0], A[:, 1] + # 直交: (h1x h2x + h1y h2y)/f^2 + h1z h2z = 0 + # 等長: (h1x^2+h1y^2 - h2x^2-h2y^2)/f^2 + (h1z^2 - h2z^2) = 0 + a = np.array([h1[0] * h2[0] + h1[1] * h2[1], h1[0] ** 2 + h1[1] ** 2 - h2[0] ** 2 - h2[1] ** 2]) + b = -np.array([h1[2] * h2[2], h1[2] ** 2 - h2[2] ** 2]) + inv_f2 = float(a @ b) / float(b @ b) # a * (1/f^2) = b ... を 1/f^2 について解く + # a/f^2 = b → 1/f^2 = (a·b)/(a·a) のほうが正しい向き + inv_f2 = float(b @ a) / float(a @ a) + return 1.0 / math.sqrt(inv_f2) if inv_f2 > 0 else None + + +def main(): + import mat_corner_detection as mcd + cap = cv2.VideoCapture(str(RUNS / RUN / "recording/top.mp4")) + ok, frame = cap.read() + cap.release() + h, w = frame.shape[:2] + corners = np.array(mcd.detect_corners(frame), np.float64) + mm = np.array(MAT_CORNERS_MM, np.float64) + H = cv2.getPerspectiveTransform(corners.astype(np.float32), mm.astype(np.float32)) # 画素 → mm + G = np.linalg.inv(H) + G /= G[2, 2] + G_m = G @ np.diag([1000.0, 1000.0, 1.0]) # m → 画素 + cx, cy = w / 2.0, h / 2.0 + f_h = focal_from_h(G_m, cx, cy) + # Unity (x, y=0, z) → 右手系 (x, y, -z) + obj = np.array([[p[0] / 1000.0, 0.0, -p[1] / 1000.0] for p in MAT_CORNERS_MM]) + hp = json.load(open(OUT / "D/home_points_unity.json")) + tips3 = np.array([[p[0], p[1], -p[2]] for p in (hp["left_tip_origin"], hp["right_tip_origin"])]) + dots = blue_dots(frame) + # 爪先の印は画像の下のほうの2つ(v が大きい2つ)。Unity の左(x 負)は画像の右(u 大) + fingers = sorted(sorted(dots, key=lambda d: -d[1])[:2], key=lambda d: -d[0]) + fingers = np.array(fingers, np.float64) + + from scipy.optimize import least_squares + bottom3 = np.array([[-0.310, 0.0, -0.361], [0.310, 0.0, -0.361]]) # 右手系(z 反転)。LR=(-310,361), LL=(310,361) + bottom_px = corners[[2, 3]] + top_px = corners[[0, 1]] # UL は x=+310 の縁、UR は x=-310 の縁 + + def unpack(p): + fv, rv, tv, zl, zr = p[0], p[1:4], p[4:7], p[7], p[8] + K = np.array([[fv, 0, cx], [0, fv, cy], [0, 0, 1]]) + return K, rv, tv, zl, zr + + def resid(p): + K, rv, tv, zl, zr = unpack(p) + pts = np.vstack([bottom3, [[0.310, 0, -zl], [-0.310, 0, -zr]], tips3]) + pr, _ = cv2.projectPoints(pts, rv, tv, K, None) + pr = pr.reshape(-1, 2) + tgt = np.vstack([bottom_px, top_px, fingers]) + return (pr - tgt).ravel() + + # 初期値: 焦点距離 1400px(iPhone の広角の目安)で、四隅から解いた姿勢 + K0 = np.array([[1400.0, 0, cx], [0, 1400.0, cy], [0, 0, 1]]) + _, rv0, tv0 = cv2.solvePnP(obj, corners, K0, None, flags=cv2.SOLVEPNP_IPPE) + p0 = np.concatenate([[1400.0], rv0.ravel(), tv0.ravel(), [-0.089, -0.089]]) + sol = least_squares(resid, p0, x_scale="jac") + K, rvec, tvec, z_ul, z_ur = unpack(sol.x) + f = float(K[0, 0]) + r = sol.fun.reshape(-1, 2) + errs = np.linalg.norm(r, axis=1) + tip_err = errs[4:6] + rvec = rvec.reshape(3, 1) + tvec = tvec.reshape(3, 1) + R, _ = cv2.Rodrigues(rvec) + proj, _ = cv2.projectPoints(obj, rvec, tvec, K, None) + reproj = np.linalg.norm(proj.reshape(-1, 2) - corners, axis=1) + C = (-R.T @ tvec).ravel() + fwd = R.T @ np.array([0, 0, 1.0]) + up = R.T @ np.array([0, -1.0, 0]) + to_u = np.array([1, 1, -1.0]) + C_u, fwd_u, up_u = C * to_u, fwd * to_u, up * to_u + fov_v = math.degrees(2 * math.atan(cy / f)) + + # Unity の LookRotation(fwd, up) を四元数にする(Unity は左手系なので、右手系の式で x,y,z を組んだ行列から求める) + zc = fwd_u / np.linalg.norm(fwd_u) + xc = np.cross(up_u, zc) # Unity: right = up × forward(左手系) + xc /= np.linalg.norm(xc) + yc = np.cross(zc, xc) + M = np.column_stack([xc, yc, zc]) + q = rot_to_quat(M) + + res = dict(run=RUN, frame=0, image_w=w, image_h=h, mat_corners_px=corners.tolist(), mat_corners_mm=MAT_CORNERS_MM, + focal_px=f, principal_px=[cx, cy], fov_vertical_deg=fov_v, + cam_pos_unity_m=C_u.tolist(), cam_forward_unity=fwd_u.tolist(), cam_up_unity=up_u.tolist(), + cam_quat_unity_xyzw=q, reproj_px=reproj.tolist(), reproj_max_px=float(reproj.max()), + rvec=rvec.ravel().tolist(), tvec=tvec.ravel().tolist(), K=K.tolist(), + focal_from_homography_only_px=f_h, + fit_points=["下の左右の隅(本当の角)", "上の2点(左右の縁の上・奥行き未知)", "HOME の爪先の印2つ"], + fit_err_px=errs.tolist(), top_points_z_mm=[float(-z_ul * 1000) * -1, float(-z_ur * 1000) * -1], blue_dots_px=[list(map(float, d)) for d in dots], + finger_dots_px=fingers.tolist(), finger_reproj_px=tip_err.tolist(), + fov_horizontal_deg=math.degrees(2 * math.atan(cx / f)), + assumption="画素は正方形・光軸は画像の中心・レンズのゆがみなし・マット四隅の4点だけで解いた") + (OUT / "D").mkdir(parents=True, exist_ok=True) + json.dump(res, open(OUT / "D/camera_fit.json", "w"), indent=1, ensure_ascii=False) + print(json.dumps({k: res[k] for k in ("focal_px", "focal_from_homography_only_px", "fov_vertical_deg", "fov_horizontal_deg", + "cam_pos_unity_m", "cam_forward_unity", "reproj_max_px", "finger_reproj_px")}, indent=1)) + + +def blue_dots(frame): + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + m = cv2.inRange(hsv, np.array([95, 120, 80]), np.array([130, 255, 255])) + cnts, _ = cv2.findContours(m, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + out = [] + for c in cnts: + if cv2.contourArea(c) > 100: + (x, y), _r = cv2.minEnclosingCircle(c) + out.append((x, y)) + return out + + +def rot_to_quat(M): + t = M[0, 0] + M[1, 1] + M[2, 2] + if t > 0: + s = math.sqrt(t + 1.0) * 2 + w = 0.25 * s + x = (M[2, 1] - M[1, 2]) / s + y = (M[0, 2] - M[2, 0]) / s + z = (M[1, 0] - M[0, 1]) / s + elif M[0, 0] > M[1, 1] and M[0, 0] > M[2, 2]: + s = math.sqrt(1.0 + M[0, 0] - M[1, 1] - M[2, 2]) * 2 + w = (M[2, 1] - M[1, 2]) / s + x = 0.25 * s + y = (M[0, 1] + M[1, 0]) / s + z = (M[0, 2] + M[2, 0]) / s + elif M[1, 1] > M[2, 2]: + s = math.sqrt(1.0 + M[1, 1] - M[0, 0] - M[2, 2]) * 2 + w = (M[0, 2] - M[2, 0]) / s + x = (M[0, 1] + M[1, 0]) / s + y = 0.25 * s + z = (M[1, 2] + M[2, 1]) / s + else: + s = math.sqrt(1.0 + M[2, 2] - M[0, 0] - M[1, 1]) * 2 + w = (M[1, 0] - M[0, 1]) / s + x = (M[0, 2] + M[2, 0]) / s + y = (M[1, 2] + M[2, 1]) / s + z = 0.25 * s + return [x, y, z, w] + + +if __name__ == "__main__": + main() diff --git a/Assets/TwinCheck/Python~/twin_d.py b/Assets/TwinCheck/Python~/twin_d.py new file mode 100644 index 0000000..cd8bb0c --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_d.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +"""依頼107 D: 実機の上カメラの1コマと、合わせた Unity のカメラの1コマを並べる。 + + shots : python3 twin_d.py shots → D/shots_d.csv(Unity で撮る場面。アーム HOME・ブロックと箱は B で求めた位置) + compose : python3 twin_d.py compose → D/d_side_by_side_384x216.png(左: 実機 / 右: Unity、どちらも 384×216) + D/d_side_by_side_960x540.png・D/d_overlay_960x540.png(重ねた確認用) +色・光・模様は調整しない(Unity はシーンの光だけで撮る)。 +""" +import csv +import json +import sys +from pathlib import Path + +import cv2 +import numpy as np + +OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所") +RUNS = Path("/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy") + + +def shots(): + cam = json.load(open(OUT / "D/camera_fit.json")) + tr = {r["run"]: r for r in csv.DictReader(open(OUT / "B/trials.csv"))}[cam["run"]] + q = cam["cam_quat_unity_xyzw"] + p = cam["cam_pos_unity_m"] + base = dict(j0=0, j1=0, j2=0, j3=0, j4=0, j5=-30, grip=0, + block_x=tr["block_x_m"], block_z=tr["block_z_m"], block_yaw=tr["block_yaw_deg"] or 0, + box_x=tr["box_x_m"], box_z=tr["box_z_m"], box_yaw=tr["box_yaw_deg"] or 0, + view="cam", target_x=0, target_z=0, cam_x=p[0], cam_y=p[1], cam_z=p[2], + cam_qx=q[0], cam_qy=q[1], cam_qz=q[2], cam_qw=q[3], cam_fov=cam["fov_vertical_deg"]) + rows = [dict(base, out=str(OUT / "D/unity_home_1920x1080.png"), w=1920, h=1080), + dict(base, out=str(OUT / "D/unity_home_384x216.png"), w=384, h=216)] + with open(OUT / "D/shots_d.csv", "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=list(rows[0].keys())) + w.writeheader() + w.writerows(rows) + + +def compose(): + cam = json.load(open(OUT / "D/camera_fit.json")) + capv = cv2.VideoCapture(str(RUNS / cam["run"] / "recording/top.mp4")) + ok, real = capv.read() + capv.release() + cv2.imwrite(str(OUT / "D/real_home_1920x1080.jpg"), real) + uni = cv2.imread(str(OUT / "D/unity_home_1920x1080.png")) + real_s = cv2.resize(real, (384, 216), interpolation=cv2.INTER_AREA) + # 学習と同じ 384×216。Unity は 1920×1080 で撮ったものを同じやり方で縮める(384×216 で直接撮ったものも残す) + uni_s = cv2.resize(uni, (384, 216), interpolation=cv2.INTER_AREA) + cv2.imwrite(str(OUT / "D/real_home_384x216.png"), real_s) + cv2.imwrite(str(OUT / "D/d_side_by_side_384x216.png"), np.hstack([real_s, uni_s])) + big = np.hstack([cv2.resize(real, (960, 540)), cv2.resize(uni, (960, 540))]) + cv2.imwrite(str(OUT / "D/d_side_by_side_960x540.png"), big) + ov = cv2.addWeighted(cv2.resize(real, (960, 540)), 0.5, cv2.resize(uni, (960, 540)), 0.5, 0) + for (u, v) in cam["mat_corners_px"]: + cv2.circle(ov, (int(u / 2), int(v / 2)), 8, (0, 200, 255), 2) + for (u, v) in cam["finger_dots_px"]: + cv2.circle(ov, (int(u / 2), int(v / 2)), 8, (255, 0, 0), 2) + cv2.imwrite(str(OUT / "D/d_overlay_960x540.png"), ov) + + +if __name__ == "__main__": + {"shots": shots, "compose": compose}[sys.argv[1]]() diff --git a/Assets/TwinCheck/Python~/twin_overlay.py b/Assets/TwinCheck/Python~/twin_overlay.py new file mode 100644 index 0000000..0f81e6f --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_overlay.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +"""依頼107 B: Unity で撮った画に、場面の名前・数字・高さの目盛り(横から見た画だけ)を書き込む。 +元の PNG は残し、書き込んだものを同じフォルダに *_note.png で置く。 + +横から見た画は正射影で、画面の中心の高さ 0.10m・上下に 0.15m(TwinCheckB.SideCenterY / SideHalfHeight)。 +""" +import csv +from pathlib import Path + +from PIL import Image, ImageDraw, ImageFont + +OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所") +FONT = "/System/Library/Fonts/ヒラギノ角ゴシック W3.ttc" +SIDE_CENTER_Y = 0.10 +SIDE_HALF_H = 0.15 +SCENE = {"1_close": "爪を閉じる命令の瞬間", "2_lift": "持ち上げたあと(いちばん高い所)", "3_box": "箱にいちばん近づいたとき"} + + +def y_px(y_m, h): + return h / 2 - (y_m - SIDE_CENTER_Y) / SIDE_HALF_H * (h / 2) + + +def main(): + rows = {r["run"]: r for r in csv.DictReader(open(OUT / "B/b_table.csv"))} + shots = list(csv.DictReader(open(OUT / "B/shots_b.csv"))) + font = ImageFont.truetype(FONT, 22) + small = ImageFont.truetype(FONT, 18) + for s in shots: + p = Path(s["out"]) + if not p.exists(): + continue + run = p.parent.name + r = rows[run] + name = p.stem.rsplit("_", 1)[0] + view = p.stem.rsplit("_", 1)[1] + im = Image.open(p).convert("RGB") + d = ImageDraw.Draw(im) + w, h = im.size + if view == "side": + for y_m, label, col in ((0.0, "マット 0mm", (40, 40, 40)), (0.024, "ブロック上面 24mm", (170, 0, 0)), + (0.043, "箱の縁 43mm(仮)", (0, 70, 170))): + yy = y_px(y_m, h) + d.line([(0, yy), (w, yy)], fill=col, width=1) + d.text((w - 230, yy - 22), label, font=small, fill=col) + for mm in range(50, 260, 50): + yy = y_px(mm / 1000, h) + d.line([(0, yy), (14, yy)], fill=(60, 60, 60), width=2) + d.text((18, yy - 11), f"{mm}mm", font=small, fill=(60, 60, 60)) + lines = [f"{run}({'失敗' if r['outcome'] == 'failure' else '成功'}) {SCENE.get(name, name)} t={float(s['t']):.1f}s {'横' if view == 'side' else '正面寄り'}", + f"爪: {'閉' if s['grip'] == '1' else '開'}"] + if name == "1_close": + lines.append(f"爪先の高さ {r['close_tip_h_mm']}mm / ブロックとの水平のずれ {r['close_tip_block_xz_mm']}mm") + elif name == "2_lift": + lines.append(f"爪先の高さ {r['lift_tip_h_mm']}mm(閉じたときから +{r['lift_rise_mm']}mm)") + else: + lines.append(f"箱の壁までの最短 {r['box_min_mm']}mm({r['box_min_part']})/ 箱の中に入った: {r['box_inside']}") + lines.append("Unity と実機の角度→位置は数十mm ずれうる。ブロックは最初の位置のまま描いている") + y = 8 + for i, t in enumerate(lines): + d.text((10, y), t, font=font if i == 0 else small, fill=(0, 0, 0)) + y += 28 if i == 0 else 23 + im.save(p.with_name(p.stem + "_note.png")) + + +if __name__ == "__main__": + main() diff --git a/Assets/TwinCheck/Python~/twin_prep.py b/Assets/TwinCheck/Python~/twin_prep.py new file mode 100644 index 0000000..eeaa06b --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_prep.py @@ -0,0 +1,370 @@ +#!/usr/bin/env python3 +"""依頼107 B の下ごしらえ(実機・daemon には触れない。記録を読むだけ)。 + +- 対象の試行を選ぶ(judgments の pick / place に人の判定が両方あるもの。失敗は全部・成功は5件) +- states.csv の event_type=actual(j0〜j5)と爪の命令(gripper_cmd)、補正の区間(stage=correct)を + 時刻の並びにして、Unity に渡す CSV(timeline.csv)にする +- top.mp4 の最初のコマから、マット四隅・赤ブロック・白箱を見つけ、マットの上の位置(Unity の m)に直す。 + 検出は r2r2r-scripts の本線と同じ関数を読み取り専用で借りる + (mat_corner_detection.detect_corners / 06f の evaluate_frame_gray と detect_box_center_by_contour) + +出力: {OUT}/B/trials.csv, {OUT}/B/timeline.csv, {OUT}/B/first_frames/{run}.jpg(検出位置に印) +""" +import csv +import json +import math +import os +import sys +from pathlib import Path + +import cv2 +import numpy as np + +RUNS = Path("/Volumes/KIOXIA/r2r2r-data/runs/pi05_policy") +OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所") +SCRIPTS = Path.home() / "r2r2r-scripts" +PLACE_MODULES = SCRIPTS / "dynamic_tracking_limits/place/place_modules" + +# 成功から比べる5件:補正なしで成功した2件と、その他の日から1件ずつ(各日のいちばん早いもの) +SUCCESS_PICK_RULE = "補正なしの成功は全部、残りは補正ありの成功を日ごとに古い日から1件ずつ" +N_SUCCESS = 5 + +MAT_CORNERS_MM = [[310, -89], [-310, -89], [-310, 361], [310, 361]] # visual_servo_pick と同じ +BLOCK_MIN_DIST_FROM_BASE_MM = 120.0 # record_teach_demo と同じ(根元のそばの赤いケーブルを拾わない) +BOX_CONTOUR_TMPL_SANITY_MAX_PX = 60 # 06f と同じ +BOX_HEIGHT_MM_ASSUMED = 43.0 # 仮の値(本線のコメントの値) + +# 時間の並びを間引く:前に残した行から全関節の差が 0.3° 未満で、2 秒以内なら捨てる +KEEP_DEG = 0.3 +KEEP_SEC = 2.0 + + +def load_detectors(): + sys.path.insert(0, str(SCRIPTS)) + sys.path.insert(0, str(PLACE_MODULES)) + import importlib + import contextlib + with contextlib.redirect_stdout(sys.stderr): + mcd = importlib.import_module("mat_corner_detection") + m06f = importlib.import_module("06f_top_white_box_detect_v7") + templates = m06f._load_white_box_templates() + return mcd, m06f, templates + + +def select_trials(): + fails, succ = [], [] + for d in sorted(os.listdir(RUNS)): + p = RUNS / d / f"{d}.json" + if not p.exists(): + continue + j = json.load(open(p)) + jd = j.get("judgments") or {} + pk, pl = jd.get("pick") or {}, jd.get("place") or {} + if not (pk.get("human_verdict") and pl.get("human_verdict")): + continue + row = dict(run=d, outcome=j.get("outcome"), pick=pk.get("human_verdict"), + place=pl.get("human_verdict"), corrections=j.get("corrections"), + model=(j.get("args") or {}).get("model", "")) + (fails if j.get("outcome") == "failure" else succ).append(row) + chosen = [s for s in succ if s["corrections"] == 0] + days = {} + for s in succ: + if s in chosen: + continue + days.setdefault(s["run"][5:13], s) + for day in sorted(days): + if len(chosen) >= N_SUCCESS: + break + chosen.append(days[day]) + return fails, chosen, len(fails) + len(succ) + + +def first_frame(run): + cap = cv2.VideoCapture(str(RUNS / run / "recording/top.mp4")) + ok, frame = cap.read() + cap.release() + return frame if ok else None + + +def px_to_mm(H, px, py): + r = cv2.perspectiveTransform(np.array([[[float(px), float(py)]]], np.float32), H) + return float(r[0][0][0]), float(r[0][0][1]) + + +def yaw_from_pts(H, pts): + """画像上の長方形の1辺を mm に直し、Unity の Y 軸まわりの向き(度、0〜90)にする。""" + a = px_to_mm(H, *pts[0]) + b = px_to_mm(H, *pts[1]) + ang = math.degrees(math.atan2(b[1] - a[1], b[0] - a[0])) + return ang % 90.0 + + +def detect_box(frame, mat_bbox, H, m06f, templates): + import contextlib + with contextlib.redirect_stdout(sys.stderr): + res = m06f.evaluate_frame_gray(frame, mat_bbox, templates) + best = res.get("overall_best") + if best is None or best.get("best_score") is None: + return None, ["箱未検出(テンプレ)"] + score = float(best["best_score"]) + tcx, tcy = best["cx"], best["cy"] + note = [] + try: + with contextlib.redirect_stdout(sys.stderr): + cr = m06f.detect_box_center_by_contour(frame, mat_bbox) + except Exception as e: # 本線と同じく fallback + cr = None + note.append(f"輪郭で例外 {type(e).__name__}") + yaw = None + if cr is not None and math.hypot(cr["cx"] - tcx, cr["cy"] - tcy) <= BOX_CONTOUR_TMPL_SANITY_MAX_PX: + cx, cy, src = cr["cx"], cr["cy"], "contour" + yaw = yaw_from_pts(H, cr["rect_pts"]) + else: + cx, cy, src = tcx, tcy, "template_fallback" + note.append("輪郭が取れずテンプレ中心") + if score < 0.5: + note.append(f"テンプレのスコアが低い({score:.2f})") + return dict(px=(cx, cy), mm=px_to_mm(H, cx, cy), score=score, source=src, yaw=yaw), note + + +def red_candidates(frame, mat_bbox, H): + """赤の塊のうち、ブロックらしいもの(根元から120mm以上・縦横比1.8以下・外接長方形の埋まり0.6以上)。""" + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + m = cv2.inRange(hsv, np.array([0, 100, 100]), np.array([10, 255, 255])) | \ + cv2.inRange(hsv, np.array([160, 100, 100]), np.array([180, 255, 255])) + m = cv2.morphologyEx(m, cv2.MORPH_OPEN, np.ones((5, 5), np.uint8)) + cnts, _ = cv2.findContours(m, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + out = [] + for c in cnts: + a = cv2.contourArea(c) + if a < 500: + continue + (cx, cy), (rw, rh), _ang = rect = cv2.minAreaRect(c) + if not (mat_bbox[0] <= cx <= mat_bbox[1] and mat_bbox[2] <= cy <= mat_bbox[3]): + continue + aspect = max(rw, rh) / max(1e-6, min(rw, rh)) + extent = a / max(1e-6, rw * rh) + mm = px_to_mm(H, cx, cy) + if math.hypot(*mm) < BLOCK_MIN_DIST_FROM_BASE_MM or aspect > 1.8 or extent < 0.6: + continue + out.append(dict(area=a, px=(cx, cy), mm=mm, yaw=yaw_from_pts(H, cv2.boxPoints(rect).tolist()))) + return out + + +def detect_block_over_frames(run, mat_bbox, H, t_states0, close_ts): + """爪を閉じる命令より前のコマを順に見て、赤がいちばん大きく見えたコマでブロックの位置を取る。 + 最初のコマでは HOME の爪の真下に半分隠れていることが多いため。""" + log = list(csv.DictReader(open(RUNS / run / "recording/top_frame_log.csv"))) + t_limit = (close_ts if close_ts is not None else 1e9) + t_states0 + cap = cv2.VideoCapture(str(RUNS / run / "recording/top.mp4")) + best = None + n_seen = 0 + for k, row in enumerate(log): + ok, f = cap.read() + if not ok: + break + if float(row["capture_ts"]) > t_limit: + break + if k % 2: + continue + cands = red_candidates(f, mat_bbox, H) + if cands: + n_seen += 1 + c = max(cands, key=lambda c: c["area"]) + if best is None or c["area"] > best["area"]: + best = dict(c, frame=k, t=float(row["capture_ts"]) - t_states0) + cap.release() + note = [] + if best is None: + return None, ["閉じる前のコマでブロックが見つからない"] + if best["area"] < 1800: + note.append(f"ブロックの見え方が小さい({best['area']:.0f}px)・隠れている疑い") + return best, note + + +def detect_box_white(frame, mat_bbox, H, tmpl_px): + """白箱を白い四角の輪郭で取る(06f の輪郭検出が1個に絞れないことが多いため、ここではテンプレ中心に + いちばん近い四角を採る)。根元から150mm以内(腕)は外す。""" + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + m = cv2.inRange(hsv, np.array([0, 0, 165]), np.array([180, 60, 255])) + mask = np.zeros_like(m) + mask[mat_bbox[2]:mat_bbox[3], mat_bbox[0]:mat_bbox[1]] = 255 + m &= mask + k = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (9, 9)) + m = cv2.morphologyEx(cv2.morphologyEx(m, cv2.MORPH_CLOSE, k), cv2.MORPH_OPEN, k) + cnts, _ = cv2.findContours(m, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + cands = [] + for c in cnts: + a = cv2.contourArea(c) + if not (15000 <= a <= 120000): + continue + rect = cv2.minAreaRect(c) + (cx, cy), (rw, rh), _ = rect + aspect = max(rw, rh) / max(1e-6, min(rw, rh)) + extent = a / max(1e-6, rw * rh) + mm = px_to_mm(H, cx, cy) + if aspect > 1.5 or extent < 0.75 or math.hypot(*mm) < 150: + continue + d = math.hypot(cx - tmpl_px[0], cy - tmpl_px[1]) if tmpl_px else 0 + cands.append(dict(px=(cx, cy), mm=mm, area=a, aspect=aspect, extent=extent, d_tmpl=d, + yaw=yaw_from_pts(H, cv2.boxPoints(rect).tolist()), pts=cv2.boxPoints(rect).tolist())) + if not cands: + return None + return min(cands, key=lambda c: c["d_tmpl"]) + + +def build_timeline(run): + rows = list(csv.DictReader(open(RUNS / run / "recording/states.csv"))) + rows.sort(key=lambda r: float(r["ts"])) + t0 = float(rows[0]["ts"]) + stage = "model" + grip = 0 + out = [] + last = None + events = [] + for r in rows: + ts = float(r["ts"]) - t0 + et = r["event_type"] + if et == "stage": + if r["stage"] != stage: + events.append(("stage", ts, r["stage"])) + stage = r["stage"] + elif et == "gripper_cmd": + g = int(float(r["gripper_state"])) + events.append(("grip", ts, g)) + grip = g + last = None # 爪の変わり目は必ず残す + elif et == "actual": + try: + j = [float(r[f"j{i}"]) for i in range(6)] + except ValueError: + continue + keep = last is None or ts - last[0] >= KEEP_SEC or \ + max(abs(a - b) for a, b in zip(j, last[1])) >= KEEP_DEG + if keep: + out.append(dict(t=ts, j=j, grip=grip, stage=stage)) + last = (ts, j) + # 補正の区間 + corr = [] + cur = None + for kind, ts, v in events: + if kind != "stage": + continue + if v == "correct" and cur is None: + cur = ts + elif v != "correct" and cur is not None: + corr.append((cur, ts)) + cur = None + if cur is not None: + corr.append((cur, out[-1]["t"] if out else cur)) + grips = [(ts, v) for kind, ts, v in events if kind == "grip"] + return out, grips, corr, t0 + + +def main(): + mcd, m06f, templates = load_detectors() + fails, succs, n_judged = select_trials() + print(f"判定がそろった試行 {n_judged} 件 / 失敗 {len(fails)} 件 / 比べる成功 {len(succs)} 件", file=sys.stderr) + (OUT / "B/first_frames").mkdir(parents=True, exist_ok=True) + + trials_rows = [] + tl_rows = [] + for tr in fails + succs: + run = tr["run"] + frame = first_frame(run) + note = [] + block = box = None + mat_bbox = None + corners = mcd.detect_corners(frame) if frame is not None else None + if corners is not None: + corners = [[float(x), float(y)] for x, y in corners] + sharp = cv2.Laplacian(cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY), cv2.CV_64F).var() if frame is not None else 0 + if sharp < 20: + note.append(f"上カメラの映像がピンぼけ(鮮明さ{sharp:.0f})で検出しない") + corners = None + if corners is None: + note.append("マット四隅を検出できず") + H = None + else: + H = cv2.getPerspectiveTransform(np.array(corners, np.float32), np.array(MAT_CORNERS_MM, np.float32)) + mat_bbox = (int(min(corners[0][0], corners[3][0])), int(max(corners[1][0], corners[2][0])), + int(min(corners[0][1], corners[1][1])), int(max(corners[2][1], corners[3][1]))) + tmpl, n2 = detect_box(frame, mat_bbox, H, m06f, templates) + note += n2 + tl, grips, corr, t0 = build_timeline(run) + close_ts = next((ts for ts, g in grips if g == 1), None) + if H is not None: + block, n1 = detect_block_over_frames(run, mat_bbox, H, t0, close_ts) + note += n1 + wb = detect_box_white(frame, mat_bbox, H, tmpl["px"] if tmpl else None) + if wb is not None: + box = dict(px=wb["px"], mm=wb["mm"], yaw=wb["yaw"], score=tmpl["score"] if tmpl else float("nan"), + source="white_contour", d_tmpl=wb["d_tmpl"], pts=wb["pts"]) + if tmpl and wb["d_tmpl"] > 80: + note.append(f"白の四角とテンプレ中心が{wb['d_tmpl']:.0f}px離れている") + elif tmpl: + box = dict(tmpl, yaw=None) + note.append("白の四角が取れずテンプレ中心(箱の角に寄りがち)") + if frame is not None: + img = frame.copy() + if corners is not None: + cv2.polylines(img, [np.array(corners, np.int32)], True, (0, 200, 255), 3) + if block: + cv2.circle(img, tuple(int(v) for v in block["px"]), 25, (0, 0, 255), 4) + if box: + cv2.circle(img, tuple(int(v) for v in box["px"]), 12, (255, 0, 0), -1) + if box.get("pts"): + cv2.polylines(img, [np.array(box["pts"], np.int32)], True, (255, 0, 0), 4) + cv2.imwrite(str(OUT / f"B/first_frames/{run}.jpg"), cv2.resize(img, (960, 540))) + trials_rows.append(dict( + run=run, outcome=tr["outcome"], human_pick=tr["pick"], human_place=tr["place"], + corrections_json=tr["corrections"], model=tr["model"], + corners_px=json.dumps([[round(x, 1), round(y, 1)] for x, y in corners]) if corners else "", + block_x_m=f"{block['mm'][0] / 1000:.4f}" if block else "", + block_z_m=f"{block['mm'][1] / 1000:.4f}" if block else "", + block_yaw_deg=f"{block['yaw']:.1f}" if block else "", + block_frame=block["frame"] if block else "", block_t=f"{block['t']:.2f}" if block else "", + block_area_px=f"{block['area']:.0f}" if block else "", + box_x_m=f"{box['mm'][0] / 1000:.4f}" if box else "", + box_z_m=f"{box['mm'][1] / 1000:.4f}" if box else "", + box_yaw_deg=f"{box['yaw']:.1f}" if box and box["yaw"] is not None else "", + box_score=f"{box['score']:.3f}" if box else "", + box_source=box["source"] if box else "", + box_height_mm_assumed=BOX_HEIGHT_MM_ASSUMED, + close_t=f"{close_ts:.2f}" if close_ts is not None else "", + n_grip_cmd=len(grips), grip_cmds=json.dumps([[round(ts, 2), g] for ts, g in grips]), + n_correct=len(corr), correct_intervals=json.dumps([[round(a, 2), round(b, 2)] for a, b in corr]), + n_samples=len(tl), duration_s=f"{tl[-1]['t']:.1f}" if tl else "", + detect_note=" / ".join(note))) + for i, s in enumerate(tl): + tl_rows.append(dict(run=run, i=i, t=f"{s['t']:.3f}", **{f"j{k}": f"{s['j'][k]:.3f}" for k in range(6)}, + grip=s["grip"], stage=s["stage"])) + print(run, tr["outcome"], "block", trials_rows[-1]["block_x_m"], trials_rows[-1]["block_z_m"], + "box", trials_rows[-1]["box_x_m"], trials_rows[-1]["box_z_m"], trials_rows[-1]["box_source"], + "samples", len(tl), note, file=sys.stderr) + + # 検出できなかった試行は、ほかの試行の中央値で仮に置く(仮であることを列に残す) + for key in ("block", "box"): + xs = sorted(float(r[f"{key}_x_m"]) for r in trials_rows if r[f"{key}_x_m"]) + zs = sorted(float(r[f"{key}_z_m"]) for r in trials_rows if r[f"{key}_z_m"]) + for r in trials_rows: + r[f"{key}_pos_source"] = "detected" + if not r[f"{key}_x_m"] and xs: + r[f"{key}_x_m"] = f"{xs[len(xs) // 2]:.4f}" + r[f"{key}_z_m"] = f"{zs[len(zs) // 2]:.4f}" + r[f"{key}_pos_source"] = "median_of_other_trials(仮)" + with open(OUT / "B/trials.csv", "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=list(trials_rows[0].keys())) + w.writeheader() + w.writerows(trials_rows) + with open(OUT / "B/timeline.csv", "w", newline="") as f: + w = csv.DictWriter(f, fieldnames=list(tl_rows[0].keys())) + w.writeheader() + w.writerows(tl_rows) + with open(OUT / "B/selection.json", "w") as f: + json.dump(dict(n_judged=n_judged, failures=[t["run"] for t in fails], + successes=[t["run"] for t in succs], success_rule=SUCCESS_PICK_RULE), f, ensure_ascii=False, indent=1) + + +if __name__ == "__main__": + main() diff --git a/Assets/TwinCheck/Python~/twin_table_b.py b/Assets/TwinCheck/Python~/twin_table_b.py new file mode 100644 index 0000000..f0e0719 --- /dev/null +++ b/Assets/TwinCheck/Python~/twin_table_b.py @@ -0,0 +1,182 @@ +#!/usr/bin/env python3 +"""依頼107 B: 時刻ごとの測定(timeline_metrics.csv)から、試行ごとの表と分類の候補、撮る場面(shots_b.csv)を作る。 + +使う場面 + 閉じた瞬間 : 爪を閉じる命令(gripper_cmd=1 の最初)にいちばん近い記録の行 + 持ち上げたあと: 閉じたあと、j0 が閉じたときから 8° 以上回る(箱へ運び始める)か、次の開く命令か、 + 記録の終わりまでのうち、爪先がいちばん高かった行 + 箱にいちばん近い: 閉じたあと(閉じていなければ全部)で、爪先・爪・前腕のどれかが箱の壁にいちばん近づいた行 + +分類の候補(Unity と実機は数十 mm ずれうるので、成功5件の値の幅と比べて大きく外れたときだけ決める) + つかむ前に外れた : 閉じた瞬間の爪先の高さか、ブロックとの水平のずれが、成功の最大より 15mm 以上大きい + 箱に当たった : 閉じたあとに、爪先・爪・前腕が箱の壁に 10mm より深くめり込む(距離 < -10mm)。 + ±10mm 以内は「縁すれすれ」として根拠の欄に書くだけにする(数 mm の差で決めない) + つかんだが持ち上げで落ちた : 上の2つに当たらず、閉じたあとに爪先が 30mm 以上持ち上がっている(人の判定は pick=no) + 分からない : 爪を閉じる命令がない・記録が足りない など +""" +import csv +import json +import math +import statistics as st +from pathlib import Path + +OUT = Path("/Volumes/KIOXIA/r2r2r-data/lab/twin/20261107_失敗の再生と置き場所") +MARGIN_MM = 15.0 +LIFT_MIN_MM = 30.0 +BOX_HIT_MM = -10.0 +BOX_NEAR_MM = 10.0 +J0_TRANSPORT_DEG = 8.0 + + +def f(v): + return float(v) if v not in ("", None) else None + + +def main(): + trials = list(csv.DictReader(open(OUT / "B/trials.csv"))) + tl = {} + for r in csv.DictReader(open(OUT / "B/timeline.csv")): + tl.setdefault(r["run"], []).append(r) + met = {} + for r in csv.DictReader(open(OUT / "B/timeline_metrics.csv")): + met.setdefault(r["run"], []).append(r) + + rows = [] + shots = [] + for tr in trials: + run = tr["run"] + T = tl.get(run, []) + M = met.get(run, []) + grips = json.loads(tr["grip_cmds"] or "[]") + corr = json.loads(tr["correct_intervals"] or "[]") + close_t = f(tr["close_t"]) + res = dict(run=run, outcome=tr["outcome"], human_pick=tr["human_pick"], human_place=tr["human_place"], + model=tr["model"].split(" ")[0], block_pos_source=tr["block_pos_source"], + box_pos_source=tr["box_pos_source"], detect_note=tr["detect_note"]) + idx_close = idx_lift = idx_box = None + if close_t is not None and M: + idx_close = min(range(len(M)), key=lambda i: abs(float(M[i]["t"]) - close_t)) + m = M[idx_close] + res.update(close_t_s=round(close_t, 1), close_tip_h_mm=f(m["tip_h_mm"]), close_tip_block_xz_mm=f(m["tip_block_xz_mm"]), + close_grip_block_mm=f(m["d_block_grip_mm"])) + j0c = float(T[idx_close]["j0"]) + next_open = next((g[0] for g in grips if g[1] == 0 and g[0] > close_t), None) + end = len(M) + for i in range(idx_close, len(M)): + if abs(float(T[i]["j0"]) - j0c) >= J0_TRANSPORT_DEG or (next_open is not None and float(M[i]["t"]) >= next_open): + end = i + break + win = range(idx_close, max(end, idx_close + 1)) + idx_lift = max(win, key=lambda i: float(M[i]["tip_h_mm"])) + res.update(lift_t_s=round(float(M[idx_lift]["t"]), 1), lift_tip_h_mm=f(M[idx_lift]["tip_h_mm"]), + lift_rise_mm=round(f(M[idx_lift]["tip_h_mm"]) - res["close_tip_h_mm"], 1)) + after = range(idx_close, len(M)) + else: + after = range(len(M)) + best = None + for i in after: + for part in ("tip", "gripper", "forearm"): + d = f(M[i][f"d_box_{part}_mm"]) + if d is not None and (best is None or d < best[0]): + best = (d, i, part) + inside_any = [i for i in after if M[i]["in_box_tip"] not in ("", "0") or M[i]["in_box_gripper"] not in ("", "0") + or M[i]["in_box_forearm"] not in ("", "0")] + if best: + idx_box = best[1] + m = M[idx_box] + res.update(box_min_mm=round(best[0], 1), box_min_part=best[2], box_min_t_s=round(float(m["t"]), 1), + box_d_tip_mm=f(m["d_box_tip_mm"]), box_d_gripper_mm=f(m["d_box_gripper_mm"]), box_d_forearm_mm=f(m["d_box_forearm_mm"])) + res["box_inside"] = "yes" if inside_any else "no" + res["box_inside_depth_mm"] = max((f(M[i]["in_box_depth_mm"]) or 0) for i in inside_any) if inside_any else 0.0 + res["n_correct"] = len(corr) + res["correct_intervals_s"] = ";".join(f"{a:.0f}-{b:.0f}" for a, b in corr) + res["correct_before_close"] = "yes" if close_t is not None and any(a < close_t for a, b in corr) else "no" + res["n_grip_close_cmds"] = sum(1 for g in grips if g[1] == 1) + res["_idx"] = (idx_close, idx_lift, idx_box) + rows.append(res) + + succ = [r for r in rows if r["outcome"] == "success" and r.get("close_tip_h_mm") is not None] + ref_h = max(r["close_tip_h_mm"] for r in succ) + ref_xz = max(r["close_tip_block_xz_mm"] for r in succ) + for r in rows: + why = [] + if r.get("close_tip_h_mm") is None: + cat = "分からない" + why.append("爪を閉じる命令の記録がない") + else: + miss_h = r["close_tip_h_mm"] > ref_h + MARGIN_MM + miss_xz = r["close_tip_block_xz_mm"] > ref_xz + MARGIN_MM + hit = r.get("box_min_mm") is not None and r["box_min_mm"] < BOX_HIT_MM + near = r.get("box_min_mm") is not None and abs(r["box_min_mm"]) <= BOX_NEAR_MM + lifted = r.get("lift_rise_mm") is not None and r["lift_rise_mm"] >= LIFT_MIN_MM + if miss_h or miss_xz: + cat = "つかむ前に外れた" + if miss_h: + why.append(f"閉じた瞬間の爪先の高さ {r['close_tip_h_mm']:.0f}mm > 成功の最大 {ref_h:.0f}+{MARGIN_MM:.0f}") + if miss_xz: + why.append(f"閉じた瞬間のブロックとの水平のずれ {r['close_tip_block_xz_mm']:.0f}mm > 成功の最大 {ref_xz:.0f}+{MARGIN_MM:.0f}") + elif hit: + cat = "箱に当たった" + why.append(f"{r['box_min_part']}が箱の壁に {-r['box_min_mm']:.0f}mm めり込む({r['box_min_t_s']}s)") + near = False + elif lifted: + cat = "つかんだが持ち上げで落ちた" + why.append(f"閉じた瞬間は成功の幅の中(高さ {r['close_tip_h_mm']:.0f}・ずれ {r['close_tip_block_xz_mm']:.0f}mm)、" + f"そのあと {r['lift_rise_mm']:.0f}mm 持ち上げ、箱には当たらない。人の判定は pick={r['human_pick']}") + else: + cat = "分からない" + why.append(f"閉じた瞬間は成功の幅の中、そのあとの持ち上げが {r.get('lift_rise_mm')}mm しかない(j0 を回し始めるまで・記録の終わりまで)") + if near: + why.append(f"{r['box_min_part']}が箱の縁すれすれ({r['box_min_mm']:+.0f}mm、{r['box_min_t_s']}s。±{BOX_NEAR_MM:.0f}mm 以内なので当たったとは決めない)") + if hit and cat != "箱に当たった": + why.append(f"(ほかに、{r['box_min_part']}が箱の壁にめり込む {-r['box_min_mm']:.0f}mm)") + # 成功は比べるためだけに載せる(分類の名前は付けない) + r["category"] = cat if r["outcome"] == "failure" else "(成功・比べる用)" + r["category_basis"] = " / ".join(why) + + cols = ["run", "outcome", "human_pick", "human_place", "model", "category", "category_basis", + "close_t_s", "close_tip_h_mm", "close_tip_block_xz_mm", "close_grip_block_mm", + "lift_t_s", "lift_tip_h_mm", "lift_rise_mm", + "box_min_mm", "box_min_part", "box_min_t_s", "box_d_tip_mm", "box_d_gripper_mm", "box_d_forearm_mm", + "box_inside", "box_inside_depth_mm", + "n_correct", "correct_intervals_s", "correct_before_close", "n_grip_close_cmds", + "block_pos_source", "box_pos_source", "detect_note"] + with open(OUT / "B/b_table.csv", "w", newline="") as fo: + w = csv.DictWriter(fo, fieldnames=cols, extrasaction="ignore") + w.writeheader() + w.writerows(rows) + + # 撮る場面 + trmap = {t["run"]: t for t in trials} + for r in rows: + tr = trmap[r["run"]] + T = tl[r["run"]] + base = dict(block_x=tr["block_x_m"], block_z=tr["block_z_m"], block_yaw=tr["block_yaw_deg"] or "0", + box_x=tr["box_x_m"], box_z=tr["box_z_m"], box_yaw=tr["box_yaw_deg"] or "0", w=960, h=720) + for name, idx, tgt, views in (("1_close", r["_idx"][0], "block", ("side", "front")), + ("2_lift", r["_idx"][1], "block", ("side",)), + ("3_box", r["_idx"][2], "box", ("side", "front"))): + if idx is None: + continue + s = T[idx] + tx, tz = (tr["block_x_m"], tr["block_z_m"]) if tgt == "block" else (tr["box_x_m"], tr["box_z_m"]) + for v in views: + shots.append(dict(base, out=str(OUT / f"B/images/{r['run']}/{name}_{v}.png"), + **{f"j{k}": s[f"j{k}"] for k in range(6)}, grip=s["grip"], view=v, + target_x=tx, target_z=tz, t=s["t"])) + with open(OUT / "B/shots_b.csv", "w", newline="") as fo: + w = csv.DictWriter(fo, fieldnames=list(shots[0].keys())) + w.writeheader() + w.writerows(shots) + + fails = [r for r in rows if r["outcome"] == "failure"] + from collections import Counter + print("成功の基準: 閉じた瞬間の高さ最大", ref_h, "ずれ最大", ref_xz) + print(Counter(r["category"] for r in fails)) + for r in rows: + print(r["run"], r["outcome"], r["category"], r.get("close_tip_h_mm"), r.get("close_tip_block_xz_mm"), + r.get("lift_rise_mm"), r.get("box_min_mm"), r.get("box_min_part"), r["box_inside"], r["n_correct"]) + + +if __name__ == "__main__": + main()