(args)
| 108 | |
| 109 | |
| 110 | def main(args): |
| 111 | |
| 112 | |
| 113 | model = mujoco.MjModel.from_xml_path(str(_XML)) |
| 114 | data = mujoco.MjData(model) |
| 115 | |
| 116 | from avp_stream import VisionProStreamer |
| 117 | |
| 118 | streamer = VisionProStreamer(ip=args.ip) |
| 119 | |
| 120 | streamer.configure_sim( |
| 121 | xml_path=str(_XML), |
| 122 | model=model, |
| 123 | data=data, |
| 124 | relative_to=[0, 0.1, 0.6, 0], |
| 125 | force_reload=False |
| 126 | ) |
| 127 | streamer.start_webrtc() |
| 128 | |
| 129 | |
| 130 | # Bodies for which to apply gravity compensation. |
| 131 | left_subtree_id = model.body("left/base_link").id |
| 132 | right_subtree_id = model.body("right/base_link").id |
| 133 | |
| 134 | # Get the dof and actuator ids for the joints we wish to control. |
| 135 | joint_names: List[str] = [] |
| 136 | velocity_limits: dict[str, float] = {} |
| 137 | for prefix in ["left", "right"]: |
| 138 | for n in _JOINT_NAMES: |
| 139 | name = f"{prefix}/{n}" |
| 140 | joint_names.append(name) |
| 141 | velocity_limits[name] = _VELOCITY_LIMITS[n] |
| 142 | dof_ids = np.array([model.joint(name).id for name in joint_names]) |
| 143 | actuator_ids = np.array([model.actuator(name).id for name in joint_names]) |
| 144 | print(actuator_ids) |
| 145 | |
| 146 | configuration = mink.Configuration(model) |
| 147 | |
| 148 | tasks = [ |
| 149 | l_ee_task := mink.FrameTask( |
| 150 | frame_name="left/gripper", |
| 151 | frame_type="site", |
| 152 | position_cost=1.0, |
| 153 | orientation_cost=0.3, |
| 154 | lm_damping=1.0, |
| 155 | ), |
| 156 | r_ee_task := mink.FrameTask( |
| 157 | frame_name="right/gripper", |
| 158 | frame_type="site", |
| 159 | position_cost=1.0, |
| 160 | orientation_cost=0.3, |
| 161 | lm_damping=1.0, |
| 162 | ), |
| 163 | posture_task := mink.PostureTask(model, cost=1e-4), |
| 164 | ] |
| 165 | |
| 166 | # Enable collision avoidance between the following geoms. |
| 167 | l_wrist_geoms = mink.get_subtree_geom_ids(model, model.body("left/wrist_link").id) |
no test coverage detected