diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp index 5243d519843b3b34678e49de7524e8fd6cebf026..3a2910e7ffffed89116bbd36f515fd0e9b24f0e7 100644 Binary files a/python/ur_simple_control/.managers.py.swp and b/python/ur_simple_control/.managers.py.swp differ diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py index 00841aefb9d36b3f1a06370cb12603afef0f7f17..fdaaa3bfe1a9e0f578914e2f045727bfef18cea8 100644 --- a/python/ur_simple_control/managers.py +++ b/python/ur_simple_control/managers.py @@ -782,7 +782,7 @@ class RobotManager: self.T_w_e_left = self.data.oMf[self.l_ee_frame_id].copy() self.T_w_e_right = self.data.oMf[self.r_ee_frame_id].copy() # NOTE: VERY EVIL, to bugfix things that depend on this like wrench (which we don't have on yumi) - self.T_w_e = self.data.oMf[self.l_ee_frame_id].copy() + #self.T_w_e = self.data.oMf[self.l_ee_frame_id].copy() def _getQd(self): """ @@ -841,19 +841,22 @@ class RobotManager: def _getWrenchInEE(self, step_called=False): - if not self.pinocchio_only: - self.wrench = np.array(self.rtde_receive.getActualTCPForce()) - self.wrench_offset + if self.robot_name != "yumi": + if not self.pinocchio_only: + self.wrench = np.array(self.rtde_receive.getActualTCPForce()) - self.wrench_offset + else: + # TODO: do something better here (at least a better distribution) + self.wrench = np.random.random(self.n_arm_joints) + if not step_called: + self._getT_w_e() + # NOTE: this mapping is equivalent to having a purely rotational action + # this is more transparent tho + mapping = np.zeros((6,6)) + mapping[0:3, 0:3] = self.T_w_e.rotation + mapping[3:6, 3:6] = self.T_w_e.rotation + self.wrench = mapping.T @ self.wrench else: - # TODO: do something better here (at least a better distribution) - self.wrench = np.random.random(self.n_arm_joints) - if not step_called: - self._getT_w_e() - # NOTE: this mapping is equivalent to having a purely rotational action - # this is more transparent tho - mapping = np.zeros((6,6)) - mapping[0:3, 0:3] = self.T_w_e.rotation - mapping[3:6, 3:6] = self.T_w_e.rotation - self.wrench = mapping.T @ self.wrench + self.wrench = np.zeros(6) def sendQd(self, qd): """