From f344323ae815aa2e09afe5fc2e41aa80127af1da Mon Sep 17 00:00:00 2001 From: peterdavidfagan Date: Wed, 13 Dec 2023 18:06:28 +0000 Subject: [PATCH] basic fixes --- mujoco_controllers/build_env.py | 2 +- mujoco_controllers/osc.py | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/mujoco_controllers/build_env.py b/mujoco_controllers/build_env.py index a63c374..f48b22d 100644 --- a/mujoco_controllers/build_env.py +++ b/mujoco_controllers/build_env.py @@ -13,7 +13,7 @@ from dm_robotics.transformations.transformations import mat_to_quat, quat_to_mat, quat_to_euler from dm_robotics.transformations import transformations as tr -from rearrangement_benchmark.env_components.props import Rectangle +from rearrangement_benchmark.environment.props import Rectangle from hydra import compose, initialize from hydra.utils import instantiate diff --git a/mujoco_controllers/osc.py b/mujoco_controllers/osc.py index cf87fbb..64b9e6c 100644 --- a/mujoco_controllers/osc.py +++ b/mujoco_controllers/osc.py @@ -22,7 +22,7 @@ from mujoco_controllers.build_env import construct_physics from mujoco_controllers.trajectory import LinearTrajectory -from rearrangement_benchmark.env_components.props import Rectangle +from rearrangement_benchmark.environment.props import Rectangle from ikpy.chain import Chain from hydra import compose, initialize from hydra.utils import instantiate @@ -277,11 +277,11 @@ def run_controller(self, duration): grasp_sensor = self.hand.grasp_sensor_callable(self.physics) gripper_converged = True if self._gripper_status == "idle" or (self._gripper_status == "closing" and grasp_sensor==2) else False - print(self.physics.named.data.qpos) + #print(self.physics.named.data.qpos) if gripper_converged: self._gripper_status = "idle" - print(grasp_sensor) - print(gripper_converged) + #print(grasp_sensor) + #print(gripper_converged) # TODO: add conditions for velocity convergence if (self.current_position_error() < self.position_threshold) and (self.current_orientation_error() < self.orientation_threshold) and (gripper_converged): converged = True @@ -309,10 +309,12 @@ def run_controller(self, duration): kinematic_chain = Chain.from_urdf_file(IKPY_URDF_PATH, base_elements=["panda_link0"]) physics, passive_view, arm, gripper = construct_physics(cfg) - + # run the controller osc = OSC(physics, arm, gripper, MOTOR_CONFIG["controllers"]["osc"], passive_view) + #print("eef_pos: ", osc.physics.bind(osc.eef_site).xpos) + #print("eef_quat: ", mat_to_quat(osc.physics.bind(osc.eef_site).xmat.reshape(3,3))) # compute the eef targets target_eef_pose = np.array([0.45,0.0,0.6]) target_orientation = R.from_euler('xyz', [0, 180, 0], degrees=True).as_matrix()