(args)
| 26 | |
| 27 | |
| 28 | def main(args): |
| 29 | model = mujoco.MjModel.from_xml_path(_XML) |
| 30 | configuration = mink.Configuration(model) |
| 31 | posture_task = mink.PostureTask(model, cost=1e-2) |
| 32 | |
| 33 | fingers = ["thumb", "first", "middle", "ring", "little"] |
| 34 | finger_tasks = [] |
| 35 | for finger in fingers: |
| 36 | task = mink.FrameTask( |
| 37 | frame_name=finger, |
| 38 | frame_type="site", |
| 39 | position_cost=1.0, |
| 40 | orientation_cost=0.0, |
| 41 | lm_damping=1.0, |
| 42 | ) |
| 43 | finger_tasks.append(task) |
| 44 | |
| 45 | tasks = [ |
| 46 | posture_task, |
| 47 | *finger_tasks, |
| 48 | ] |
| 49 | |
| 50 | model = configuration.model |
| 51 | data = configuration.data |
| 52 | solver = "daqp" |
| 53 | |
| 54 | from avp_stream import VisionProStreamer |
| 55 | streamer = VisionProStreamer(ip=args.ip) |
| 56 | |
| 57 | streamer.configure_sim( |
| 58 | xml_path=str(_XML), |
| 59 | model=model, |
| 60 | data=data, |
| 61 | relative_to=[-0.3, 0.1, 0.8, 90], |
| 62 | ) |
| 63 | |
| 64 | streamer.start_webrtc() |
| 65 | |
| 66 | |
| 67 | # configuration.update_from_keyframe("grasp hard") |
| 68 | |
| 69 | # Initialize mocap bodies at their respective sites. |
| 70 | posture_task.set_target_from_configuration(configuration) |
| 71 | for finger in fingers: |
| 72 | mink.move_mocap_to_frame(model, data, f"{finger}_target", finger, "site") |
| 73 | |
| 74 | rate = RateLimiter(frequency=500.0, warn=False) |
| 75 | dt = rate.dt |
| 76 | t = 0 |
| 77 | |
| 78 | while True: |
| 79 | hand = streamer.get_latest() |
| 80 | |
| 81 | rot = np.eye(4) |
| 82 | rot[:3, :3] = R.from_euler("xz", [0, 180], degrees=True).as_matrix() |
| 83 | rot = rot[np.newaxis, :, :] |
| 84 | |
| 85 | smaller_hand = hand["left_fingers"].copy() |
no test coverage detected