diff --git a/deoxys/deoxys/utils/ik_utils.py b/deoxys/deoxys/utils/ik_utils.py index 34e0ea6..ed67690 100644 --- a/deoxys/deoxys/utils/ik_utils.py +++ b/deoxys/deoxys/utils/ik_utils.py @@ -18,7 +18,7 @@ def ik_trajectory_from_T_seq(self, T_seq, start_joint_positions, verbose=True): assert(len(start_joint_positions) == 7), "start_joint_positions should be a list of 7 elements" predicted_joints_seq = [np.array(start_joint_positions)] - self.data.qpos[:] = start_joint_positions + [0.04] * 2 + self.data.qpos[:] = np.concatenate((start_joint_positions, [0.04] * 2)) gripper_site_id = self.model.site("grip_site").id jac = np.zeros((6, self.model.nv)) @@ -68,7 +68,7 @@ def ik_trajectory_delta_position(self, delta_pos, start_joint_positions, num_poi assert(len(start_joint_positions) == 7), "start_joint_positions should be a list of 7 elements" predicted_joints_seq = [np.array(start_joint_positions)] - self.data.qpos[:] = start_joint_positions + [0.04] * 2 + self.data.qpos[:] = np.concatenate((start_joint_positions, [0.04] * 2)) gripper_site_id = self.model.site("grip_site").id jac = np.zeros((6, self.model.nv)) @@ -84,7 +84,7 @@ def ik_trajectory_to_target_position(self, target_pos, start_joint_positions, nu assert(len(start_joint_positions) == 7), "start_joint_positions should be a list of 7 elements" predicted_joints_seq = [np.array(start_joint_positions)] - self.data.qpos[:] = start_joint_positions + [0.04] * 2 + self.data.qpos[:] = np.concatenate((start_joint_positions, [0.04] * 2)) gripper_site_id = self.model.site("grip_site").id jac = np.zeros((6, self.model.nv)) @@ -124,7 +124,7 @@ def simulate_joint_sequence(self, joint_sequence, loop=False, fps=30, render=Tru mujoco.mjv_defaultFreeCamera(self.model, viewer.cam) while viewer.is_running(): for joint_conf in joint_sequence: - self.data.qpos[:] = joint_conf.tolist() + [0.04] * 2 + self.data.qpos[:] = np.concatenate((joint_conf, [0.04] * 2)) mujoco.mj_step(self.model, self.data, 1) viewer.sync() time.sleep(1/fps) @@ -139,7 +139,7 @@ def simulate_joint_sequence(self, joint_sequence, loop=False, fps=30, render=Tru break else: for joint_conf in joint_sequence: - self.data.qpos[:] = joint_conf.tolist() + [0.04] * 2 + self.data.qpos[:] = np.concatenate((joint_conf, [0.04] * 2)) mujoco.mj_step(self.model, self.data, 1) gripper_site_id = self.model.site("grip_site").id # mujoco.mj_fwdPosition(model, data) @@ -166,7 +166,7 @@ def inverse_kinematics(self, model, data, target_mat, target_pos, reset_joint_po site_xquat = np.empty(4, dtype=dtype) neg_site_xquat = np.empty(4, dtype=dtype) error_rot_quat = np.empty(4, dtype=dtype) - data.qpos[:] = reset_joint_positions + [0.04] * 2 + data.qpos[:] = np.concatenate((reset_joint_positions ,[0.04] * 2)) mujoco.mj_fwdPosition(model, data) gripper_site_id = model.site("grip_site").id @@ -245,4 +245,4 @@ def interpolate_dense_traj(self, joint_seq, minimal_displacement=0.005): new_joint_seq.append(new_joint) new_joint_seq.append(joint_seq[-1]) print("increased joint sequence from {} to {}".format(len(joint_seq), len(new_joint_seq))) - return new_joint_seq + return new_joint_seq \ No newline at end of file