diff --git a/domain/environment/DClawEnvironment.py b/domain/environment/DClawEnvironment.py index 3904d85..ea522fe 100644 --- a/domain/environment/DClawEnvironment.py +++ b/domain/environment/DClawEnvironment.py @@ -54,6 +54,7 @@ def __init__(self, config): self._valve_jnt_id = self.model.joint_name2id('valve_OBJRx') self._target_bid = self.model.body_name2id('target') + self._contact_bid = self.model.body_name2id('vis_contact_body') self._target_sid = self.model.site_name2id('tmark') self._target_position = None self.sim = None @@ -462,14 +463,19 @@ def step_with_inplicit_step(self): for i in range(self.inplicit_step): self.sim.step() + self.viewer.vopt.flags[mujoco_py.const.VIS_CONTACTFORCE] = 1 # print("self.sim.data.ncon: ", self.sim.data.ncon) for i in range(self.sim.data.ncon): con = self.sim.data.contact[i] # if con.geom1 == self.sim.model.geom_name2id("phy_tip") and con.geom2 == self.sim.model.geom_name2id("phy_valve_6_oclock"): if con.geom1: - print(con.geom1) - print(con.geom2) + # print(con.geom1) + # print(con.geom2) + # contact_pos = con.pos + self.sim.model.body_pos[self._contact_bid][:] = con.pos + if self.sim.data.ncon > 1: contact_pos = con.pos + def step(self): self.sim.step() \ No newline at end of file diff --git a/domain/environment/model/dclaw3xh_valve3_default.xml b/domain/environment/model/dclaw3xh_valve3_default.xml index 0575ae1..ed2007b 100644 --- a/domain/environment/model/dclaw3xh_valve3_default.xml +++ b/domain/environment/model/dclaw3xh_valve3_default.xml @@ -25,5 +25,9 @@ + + + + diff --git a/domain/environment/model/robot/assets/claw_unit.xml b/domain/environment/model/robot/assets/claw_unit.xml new file mode 100644 index 0000000..803b43a --- /dev/null +++ b/domain/environment/model/robot/assets/claw_unit.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/usecase/DemoRobotMove_with_mujoco_viewer.py b/usecase/DemoRobotMove_with_mujoco_viewer.py index 1eb0224..308b979 100644 --- a/usecase/DemoRobotMove_with_mujoco_viewer.py +++ b/usecase/DemoRobotMove_with_mujoco_viewer.py @@ -28,10 +28,10 @@ def run(self, config): ctrl[:, 1] = np.linspace(0, np.pi*0.1, step) ctrl[:, 2] = np.linspace(0, np.pi*0.25, step) - for s in range(10): + for s in range(30): env.reset(state) - # env.canonicalize_texture() # canonicalテクスチャを設定 - env.randomize_texture() # randomテクスチャを設定 + env.canonicalize_texture() # canonicalテクスチャを設定 + # env.randomize_texture() # randomテクスチャを設定 for i in range(step): env.set_ctrl(ctrl[i]) env.render_with_viewer() # Mujocoのビューワを使って描画